src/multibody/pool/model.hpp

Go to the documentation of this file.

//
// 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;
    
    explicit 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)
    {}
    
    ModelPoolTpl(const ModelPoolTpl & pool_model)
    : m_model(pool_model.m_model)
    , m_datas(pool_model.m_datas)
    , m_size(pool_model.m_size)
    {}
    
    const Model & model() const { return m_model; }
    
    Model & model() { return m_model; }
    
    void update(const Model & model)
    {
      m_model = model;
      update(Data(model));
    }
    
    void update(const Data & data)
    {
      std::fill(m_datas.begin(),m_datas.end(),data);
    }

    void update(const Model & model,
                const Data & data)
    {
      m_model = model;
      update(data);
    }
    
    int size() const { return m_size; }
    
    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, (long)(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;
    }

    const DataVector & datas() const { return m_datas; }
    
    DataVector & datas() { return m_datas; }
    
    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];
    }
    
    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];
    }
    
    virtual ~ModelPoolTpl() {};
    
  protected:
    
    Model m_model;
    
    DataVector m_datas;
    
    int m_size;
    
    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__