Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/multibody/visitor/joint-binary-visitor.hpp	Lines:	43	43	100.0 %
Date:	2024-01-23 21:41:47	Branches:	16	32	50.0 %


//
// Copyright (c) 2019 INRIA
//

#ifndef __pinocchio_multibody_visitior_joint_binary_visitor_hpp__
#define __pinocchio_multibody_visitior_joint_binary_visitor_hpp__

#include <boost/variant.hpp>

#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/visitor/fusion.hpp"

namespace pinocchio
{
  namespace fusion
  {
    namespace bf = boost::fusion;

    typedef boost::blank NoArg;

    ///
    /// \brief Base structure for \b Binary  visitation of two JointModels.
    ///        This structure provides runners to call the right visitor according to the number of arguments.
    ///        This should be used when deriving new rigid body algorithms.
    ///
    template<typename JointVisitorDerived, typename ReturnType = void>
    struct JointBinaryVisitorBase
    {

      template<typename JointModelDerived1, typename JointModelDerived2, typename ArgsTmp>
      static ReturnType run(const JointModelBase<JointModelDerived1> & jmodel1,
                            const JointModelBase<JointModelDerived2> & jmodel2,
                            typename JointModelBase<JointModelDerived1>::JointDataDerived & jdata1,
                            typename JointModelBase<JointModelDerived2>::JointDataDerived & jdata2,
                            ArgsTmp args)
      {
        InternalVisitorModelAndData<JointModelDerived1,JointModelDerived2,ArgsTmp> visitor(jdata1,jdata2,args);
        return visitor(jmodel1.derived(),jmodel2.derived());
      }

      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ArgsTmp>
      static ReturnType run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel1,
                            const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel2,
                            JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata1,
                            JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata2,
                            ArgsTmp args)
      {
        typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
        InternalVisitorModelAndData<JointModel,JointModel,ArgsTmp> visitor(jdata1,jdata2,args);
        return boost::apply_visitor(visitor,jmodel1,jmodel2);
      }

      template<typename JointModelDerived1, typename JointModelDerived2>
      static ReturnType run(const JointModelBase<JointModelDerived1> & jmodel1,
                            const JointModelBase<JointModelDerived2> & jmodel2,
                            typename JointModelBase<JointModelDerived1>::JointDataDerived & jdata1,
                            typename JointModelBase<JointModelDerived2>::JointDataDerived & jdata2)
      {
        InternalVisitorModelAndData<JointModelDerived1,JointModelDerived2,NoArg> visitor(jdata1,jdata2);
        return visitor(jmodel1.derived(),jmodel2.derived());
      }

      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
      static ReturnType run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel1,
                            const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel2,
                            JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata1,
                            JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata2)
      {
        typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
        InternalVisitorModelAndData<JointModel,JointModel,NoArg> visitor(jdata1,jdata2);
        return boost::apply_visitor(visitor,jmodel1,jmodel2);
      }

      template<typename JointModelDerived1, typename JointModelDerived2, typename ArgsTmp>
      static ReturnType run(const JointModelBase<JointModelDerived1> & jmodel1,
                            const JointModelBase<JointModelDerived2> & jmodel2,
                            ArgsTmp args)
      {
        InternalVisitorModel<ArgsTmp> visitor(args);
        return visitor(jmodel1.derived(),jmodel2.derived());
      }

      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ArgsTmp>
      static ReturnType run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel1,
                            const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel2,
                            ArgsTmp args)
      {
        InternalVisitorModel<ArgsTmp> visitor(args);
        return boost::apply_visitor(visitor,jmodel1,jmodel2);
      }

      template<typename JointModelDerived1, typename JointModelDerived2>
      static ReturnType run(const JointModelBase<JointModelDerived1> & jmodel1,
                            const JointModelBase<JointModelDerived2> & jmodel2)
      {
        InternalVisitorModel<NoArg> visitor;
        return visitor(jmodel1.derived(),jmodel2.derived());
      }

      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
      static ReturnType run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel1,
                            const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel2)
      {
        InternalVisitorModel<NoArg> visitor;
        return boost::apply_visitor(visitor,jmodel1,jmodel2);
      }

    private:

      template<typename JointModel1, typename JointModel2, typename ArgType>
      struct InternalVisitorModelAndData
      : public boost::static_visitor<ReturnType>
      {
        typedef typename JointModel1::JointDataDerived JointData1;
        typedef typename JointModel2::JointDataDerived JointData2;

        InternalVisitorModelAndData(JointData1 & jdata1,
                                    JointData2 & jdata2,
                                    ArgType args)
        : jdata1(jdata1)
        , jdata2(jdata2)
        , args(args)
        {}

        template<typename JointModelDerived1, typename JointModelDerived2>
        ReturnType operator()(const JointModelBase<JointModelDerived1> & jmodel1,
                              const JointModelBase<JointModelDerived2> & jmodel2) const
        {
          return bf::invoke(&JointVisitorDerived::template algo<JointModelDerived1,JointModelDerived2>,
                            bf::append(boost::ref(jmodel1.derived()),
                                       boost::ref(jmodel2.derived()),
                                       boost::ref(boost::get<typename JointModelBase<JointModelDerived1>::JointDataDerived >(jdata1)),
                                       boost::ref(boost::get<typename JointModelBase<JointModelDerived2>::JointDataDerived >(jdata2)),
                                       args));
        }

        JointData1 & jdata1;
        JointData2 & jdata2;

        ArgType args;
      };

      template<typename JointModel1, typename JointModel2>
      struct InternalVisitorModelAndData<JointModel1,JointModel2,NoArg>
      : public boost::static_visitor<ReturnType>
      {
        typedef typename JointModel1::JointDataDerived JointData1;
        typedef typename JointModel2::JointDataDerived JointData2;

        InternalVisitorModelAndData(JointData1 & jdata1, JointData2 & jdata2)
        : jdata1(jdata1)
        , jdata2(jdata2)
        {}

        template<typename JointModelDerived1, typename JointModelDerived2>
        ReturnType operator()(const JointModelBase<JointModelDerived1> & jmodel1,
                              const JointModelBase<JointModelDerived2> & jmodel2) const
        {
          return bf::invoke(&JointVisitorDerived::template algo<JointModelDerived1,JointModelDerived2>,
                            bf::make_vector(boost::ref(jmodel1.derived()),
                                            boost::ref(jmodel2.derived()),
                                            boost::ref(boost::get<typename JointModelBase<JointModelDerived1>::JointDataDerived >(jdata1)),
                                            boost::ref(boost::get<typename JointModelBase<JointModelDerived2>::JointDataDerived >(jdata2))));
        }

        JointData1 & jdata1;
        JointData2 & jdata2;
      };

      template<typename ArgType, typename Dummy = void>
      struct InternalVisitorModel
      : public boost::static_visitor<ReturnType>
      {
        InternalVisitorModel(ArgType args)
        : args(args)
        {}

        template<typename JointModel1, typename JointModel2>
        ReturnType operator()(const JointModelBase<JointModel1> & jmodel1,
                              const JointModelBase<JointModel2> & jmodel2) const
        {
          return bf::invoke(&JointVisitorDerived::template algo<JointModel1,JointModel2>,
                            bf::append(boost::ref(jmodel1.derived()),
                                       boost::ref(jmodel2.derived()),
                                       args));
        }

        ArgType args;
      };

      template<typename Dummy>
      struct InternalVisitorModel<NoArg,Dummy>
      : public boost::static_visitor<ReturnType>
      {
        InternalVisitorModel() {}

        template<typename JointModel1, typename JointModel2>
        ReturnType operator()(const JointModelBase<JointModel1> & jmodel1,
                              const JointModelBase<JointModel2> & jmodel2) const
        {
          return JointVisitorDerived::
          template algo<JointModel1,JointModel2>(jmodel1.derived(),
                                                 jmodel2.derived());
        }
      };

    }; // struct JointBinaryVisitorBase

  } // namespace fusion
} // namespace pinocchio

#endif // ifndef __pinocchio_multibody_visitior_joint_binary_visitor_hpp__


Generated by: GCOVR (Version 4.2)

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__