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 ::pinocchio::GeometryModel GeometryModel;
    typedef ::pinocchio::GeometryData GeometryData;
    
    typedef std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > GeometryModelVector;
    typedef std::vector<GeometryData,Eigen::aligned_allocator<GeometryData> > GeometryDataVector;
    
    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))
    {}
    
    GeometryPoolTpl(const GeometryPoolTpl & other)
    : Base(other)
    , m_geometry_model(other.m_geometry_model)
    , m_geometry_datas(other.m_geometry_datas)
    {}
    
    const GeometryModel & geometry_model() const { return m_geometry_model; }
    
    GeometryModel & geometry_model() { return m_geometry_model; }
    
    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];
    }
    
    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];
    }
    
    const GeometryDataVector & geometry_datas() const { return m_geometry_datas; }
    
    GeometryDataVector & geometry_datas() { return m_geometry_datas; }
    
    using Base::update;
    using Base::size;
    using Base::model;
    using Base::datas;
    
    void update(const GeometryData & geometry_data)
    {
      std::fill(m_geometry_datas.begin(),m_geometry_datas.end(),geometry_data);
    }
    
    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));
    }
    
    void update(const GeometryModel & geometry_model,
                const GeometryData & geometry_data)
    {
      m_geometry_model = geometry_model;
      update(geometry_data);
    }
    
    virtual ~GeometryPoolTpl() {};
    
  protected:
    
    GeometryModel m_geometry_model;
    
    GeometryDataVector m_geometry_datas;
      
    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, (long)(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__