core: add {Model,Geometry}Pool

CMakeLists.txt

@@ -203,6 +203,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/spatial")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/joint")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/liegroup")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/pool")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/visitor")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/urdf")

src/multibody/pool/geometry.hpp

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef __pinocchio_multibody_pool_geometry_hpp__
+#define __pinocchio_multibody_pool_geometry_hpp__
+
+#include "pinocchio/multibody/geometry.hpp"
+#include "pinocchio/multibody/pool/model.hpp"
+
+#include "pinocchio/utils/openmp.hpp"
+
+namespace pinocchio
+{
+  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
+  class GeometryPoolTpl
+  : public ModelPoolTpl<_Scalar,_Options,JointCollectionTpl>
+  {
+  public:
+    
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    
+    typedef ModelPoolTpl<_Scalar,_Options,JointCollectionTpl> Base;
+    typedef _Scalar Scalar;
+    enum { Options = _Options };
+    
+    typedef typename Base::Model Model;
+    typedef typename Base::Data Data;
+    typedef typename Base::DataVector DataVector;
+    typedef GeometryModel GeometryModel;
+    typedef GeometryData GeometryData;
+    
+    typedef std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > GeometryModelVector;
+    typedef std::vector<GeometryData,Eigen::aligned_allocator<GeometryData> > GeometryDataVector;
+    
+    /// \brief Default constructor from a model and a pool size.
+    ///
+    /// \param[in] model input model used for parallel computations.
+    /// \param[in] geometry_model input geometry model used for parallel computations.
+    /// \param[in] pool_size total size of the pool.
+    ///
+    GeometryPoolTpl(const Model & model, const GeometryModel & geometry_model,
+                    const int pool_size = omp_get_max_threads())
+    : Base(model,pool_size)
+    , m_geometry_model(geometry_model)
+    , m_geometry_datas((size_t)pool_size,GeometryData(geometry_model))
+    {}
+    
+    /// \brief Copy constructor from an other GeometryPoolTpl.
+    ///
+    /// \param[in] other GeometryPoolTpl to copy.
+    ///
+    GeometryPoolTpl(const GeometryPoolTpl & other)
+    : Base(other)
+    , m_geometry_model(other.m_geometry_model)
+    , m_geometry_datas(other.m_geometry_datas)
+    {}
+    
+    /// \brief Returns the geometry model
+    const GeometryModel & geometry_model() const { return m_geometry_model; }
+    
+    /// \brief Returns the geometry model
+    GeometryModel & geometry_model() { return m_geometry_model; }
+    
+    /// \brief Returns the geometry_data at index
+    const GeometryData & geometry_data(const size_t index) const
+    {
+      PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_geometry_datas.size(),
+                                     "Index greater than the size of the geometry_datas vector.");
+      return m_geometry_datas[index];
+    }
+    
+    /// \brief Returns the geometry_data at index
+    GeometryData & geometry_data(const size_t index)
+    {
+      PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_geometry_datas.size(),
+                                     "Index greater than the size of the geometry_datas vector.");
+      return m_geometry_datas[index];
+    }
+    
+    /// \brief Vector of Geometry Data
+    const GeometryDataVector & geometry_datas() const { return m_geometry_datas; }
+    
+    /// \brief Vector of Geometry Data
+    GeometryDataVector & geometry_datas() { return m_geometry_datas; }
+    
+    using Base::update;
+    using Base::size;
+    using Base::model;
+    using Base::datas;
+    
+    ///
+    /// \brief Update the geometry datas with the new value
+    ///
+    /// \param[in] geometry_data new geometry data value
+    ///
+    void update(const GeometryData & geometry_data)
+    {
+      std::fill(m_geometry_datas.begin(),m_geometry_datas.end(),geometry_data);
+    }
+    
+    ///
+    /// \brief Update the geometry model with the new input value.
+    ///        In this case, all the geometry_datas will be replaced
+    ///
+    /// \param[in] geometry_model new geometry model value.
+    ///
+    void update(const GeometryModel & geometry_model)
+    {
+      m_geometry_model = geometry_model;
+      std::fill(m_geometry_datas.begin(),m_geometry_datas.end(),
+                GeometryData(m_geometry_model));
+    }
+    
+    /// \brief Destructor
+    virtual ~GeometryPoolTpl() {};
+    
+  protected:
+    
+    /// \brief Geometry Model associated to the pool.
+    GeometryModel m_geometry_model;
+    
+    /// \brief Vector of Geometry Data associated to the pool.
+    GeometryDataVector m_geometry_datas;
+      
+    /// \brief Method to implement in the derived classes.
+    virtual void do_resize(const int new_size)
+    {
+      m_geometry_datas.resize((size_t)new_size);
+      if(size() < new_size)
+      {
+        typename GeometryDataVector::iterator it = m_geometry_datas.begin();
+        std::advance(it, (size_t)(new_size - size()));
+        std::fill(it,m_geometry_datas.end(),m_geometry_datas[0]);
+      }
+    }
+    
+  };
+
+  typedef GeometryPoolTpl<double,0,JointCollectionDefaultTpl> GeometryPool;
+}
+
+#endif // ifndef __pinocchio_multibody_pool_geometry_hpp__

src/multibody/pool/model.hpp

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef __pinocchio_multibody_pool_model_hpp__
+#define __pinocchio_multibody_pool_model_hpp__
+
+#include <algorithm>
+#include <omp.h>
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/data.hpp"
+
+#include "pinocchio/utils/openmp.hpp"
+
+namespace pinocchio
+{
+  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
+  class ModelPoolTpl
+  {
+  public:
+    
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    
+    typedef _Scalar Scalar;
+    enum { Options = _Options };
+    
+    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
+    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
+    
+    typedef std::vector<Model,Eigen::aligned_allocator<Model> > ModelVector;
+    typedef std::vector<Data,Eigen::aligned_allocator<Data> > DataVector;
+    
+    /// \brief Default constructor from a model and a pool size.
+    ///
+    /// \param[in] model input model used for parallel computations.
+    /// \param[in] pool_size total size of the pool.
+    ///
+    ModelPoolTpl(const Model & model,
+                 const int pool_size = omp_get_max_threads())
+    : m_model(model)
+    , m_datas((size_t)pool_size, Data(model))
+    , m_size(pool_size)
+    {}
+    
+    /// \brief Copy constructor from an other PoolModel.
+    ///
+    /// \param[in] pool_model PoolModel to copy.
+    ///
+    ModelPoolTpl(const ModelPoolTpl & pool_model)
+    : m_model(pool_model.m_model)
+    , m_datas(pool_model.m_datas)
+    , m_size(pool_model.m_size)
+    {}
+    
+    /// \brief Returns the model stored within the pool.
+    const Model & model() const { return m_model; }
+    
+    /// \brief Returns the model stored within the pool.
+    Model & model() { return m_model; }
+    
+    /// \brief Update the model, meaning that all the datas will be refreshed accordingly.
+    ///
+    /// \param[in] model new model value.
+    ///
+    void update(const Model & model)
+    {
+      m_model = model;
+      update(Data(model));
+    }
+    
+    /// \brief Update all the datas with the input data value.
+    ///
+    /// \param[in] data new value to use and to copy within the vector of data.
+    ///
+    void update(const Data & data)
+    {
+      std::fill(m_datas.begin(),m_datas.end(),data);
+    }
+    
+    /// \brief Returns the size of the pool.
+    int size() const { return m_size; }
+    
+    /// \brief Set the size of the pool and perform the appropriate resize.
+    void resize(const int new_size)
+    {
+      m_datas.resize((size_t)new_size);
+      if(m_size < new_size)
+      {
+        typename DataVector::iterator it = m_datas.begin();
+        std::advance(it, (size_t)(new_size - m_size));
+        std::fill(it,m_datas.end(),m_datas[0]);
+      }
+      do_resize(new_size); // call Derived::do_resize();
+      m_size = new_size;
+    }
+
+    /// \brief Returns the data vectors
+    const DataVector & datas() const { return m_datas; }
+    
+    /// \brief Returns the data vectors
+    DataVector & datas() { return m_datas; }
+    
+    /// \brief Return a specific data
+    const Data & data(const size_t index) const
+    {
+      PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_datas.size(),
+                                     "Index greater than the size of the datas vector.");
+      return m_datas[index];
+    }
+    
+    /// \brief Returns a specific data
+    Data & data(const size_t index)
+    {
+      PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_datas.size(),
+                                     "Index greater than the size of the datas vector.");
+      return m_datas[index];
+    }
+    
+    /// \brief Destructor
+    virtual ~ModelPoolTpl() {};
+    
+  protected:
+    
+    /// \brief Model stored within the pool.
+    Model m_model;
+    
+    /// \brief Vector of data elements
+    DataVector m_datas;
+    
+    /// \brief Number of threads used for parallel computations
+    int m_size;
+    
+    /// \brief Method to implement in the derived classes.
+    virtual void do_resize(const int new_size)
+    {
+      PINOCCHIO_UNUSED_VARIABLE(new_size);
+    }
+    
+  };
+
+  typedef ModelPoolTpl<double,0,JointCollectionDefaultTpl> ModelPool;
+}
+
+#endif // ifndef __pinocchio_multibody_pool_model_hpp__