Program Listing for File model.hpp
↰ Return to documentation for file (include/pinocchio/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;
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__