joint-binary-visitor.hpp

//
// Copyright (c) 2019 INRIA
//
 
#ifndef __pinocchio_multibody_visitor_joint_binary_visitor_hpp__
#define __pinocchio_multibody_visitor_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;
 
    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_visitor_joint_binary_visitor_hpp__