geometry.hpp

//
// Copyright (c) 2021-2022 INRIA
//

#ifndef __pinocchio_multibody_pool_geometry_hpp__
#define __pinocchio_multibody_pool_geometry_hpp__

#include "pinocchio/multibody/geometry.hpp"
#include "pinocchio/multibody/pool/model.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::ModelVector ModelVector;
 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 size_t pool_size = (size_t)omp_get_max_threads())
 : Base(model, pool_size)
 {
 m_geometry_models.reserve(pool_size);
 m_geometry_datas.reserve(pool_size);
 for (size_t k = 0; k < pool_size; ++k)
 {
 m_geometry_models.push_back(geometry_model.clone());
 m_geometry_datas.push_back(GeometryData(m_geometry_models[k]));

 typedef typename GeometryData::SE3 SE3;
 for (SE3 & oMg_i : m_geometry_datas.back().oMg)
 {
 oMg_i.setIdentity();
 }
 }
 }

 GeometryPoolTpl(const GeometryPoolTpl & other)
 : Base(other)
 {
 const size_t pool_size = other.size();
 m_geometry_models.reserve(pool_size);
 m_geometry_datas.reserve(pool_size);
 for (size_t k = 0; k < pool_size; ++k)
 {
 m_geometry_models.push_back(other.m_geometry_models[k].clone());
 m_geometry_datas.push_back(GeometryData(m_geometry_models[k]));
 }
 }

 const GeometryModel & getGeometryModel(const size_t index) const
 {
 PINOCCHIO_CHECK_INPUT_ARGUMENT(
 index < m_geometry_models.size(),
 "Index greater than the size of the geometry_models vector.");
 return m_geometry_models[index];
 }

 GeometryModel & getGeometryModel(const size_t index)
 {
 PINOCCHIO_CHECK_INPUT_ARGUMENT(
 index < m_geometry_models.size(),
 "Index greater than the size of the geometry_models vector.");
 return m_geometry_models[index];
 }

 const GeometryData & getGeometryData(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 & getGeometryData(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 & getGeometryDatas() const
 {
 return m_geometry_datas;
 }

 GeometryDataVector & getGeometryDatas()
 {
 return m_geometry_datas;
 }

 const GeometryModelVector & getGeometryModels() const
 {
 return m_geometry_models;
 }

 GeometryModelVector & getGeometryModels()
 {
 return m_geometry_models;
 }

 using Base::size;
 using Base::update;

 void sync(const GeometryModel & geometry_model, const std::vector<GeomIndex> & geometry_indexes)
 {
 for (GeomIndex i : geometry_indexes)
 PINOCCHIO_CHECK_INPUT_ARGUMENT(
 i < geometry_model.ngeoms,
 "One of the given geometry index is greater than geometry_model.ngeoms.");

 for (GeometryModel & geometry_model_pool : m_geometry_models)
 {
 for (GeomIndex i : geometry_indexes)
 geometry_model_pool.geometryObjects[i] = geometry_model.geometryObjects[i].clone();
 }
 }

 virtual void update(const GeometryData & geometry_data_to_copy)
 {
 for (GeometryData & geometry_data : m_geometry_datas)
 {
 geometry_data.oMg = geometry_data_to_copy.oMg;
 geometry_data.activeCollisionPairs = geometry_data_to_copy.activeCollisionPairs;
 geometry_data.distanceRequests = geometry_data_to_copy.distanceRequests;
 geometry_data.collisionRequests = geometry_data_to_copy.collisionRequests;
 geometry_data.collisionPairIndex = geometry_data_to_copy.collisionPairIndex;
 }
 }

 virtual ~GeometryPoolTpl() {};

 protected:
 GeometryModelVector m_geometry_models;

 GeometryDataVector m_geometry_datas;

 virtual void doResize(const size_t new_size)
 {
 const size_t current_size = (size_t)size();
 m_geometry_models.resize((size_t)new_size);
 m_geometry_datas.resize((size_t)new_size);
 if (current_size < new_size)
 {
 for (size_t k = current_size; k < new_size; ++k)
 {
 m_geometry_models[k] = m_geometry_models[0].clone();
 m_geometry_datas[k] = GeometryData(m_geometry_models[k]);
 }
 }
 }
 };

} // namespace pinocchio

#endif // ifndef __pinocchio_multibody_pool_geometry_hpp__