broadphase-manager.hpp

//
// Copyright (c) 2022 INRIA
//

#ifndef __pinocchio_collision_pool_broadphase_manager_hpp__
#define __pinocchio_collision_pool_broadphase_manager_hpp__

#include <omp.h>

#include "pinocchio/multibody/pool/geometry.hpp"
#include "pinocchio/collision/broadphase-manager.hpp"

namespace pinocchio
{

 template<
 typename _BroadPhaseManagerDerived,
 typename _Scalar,
 int _Options,
 template<typename, int> class JointCollectionTpl>
 class BroadPhaseManagerPoolBase : public GeometryPoolTpl<_Scalar, _Options, JointCollectionTpl>
 {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _BroadPhaseManagerDerived BroadPhaseManager;
 typedef GeometryPoolTpl<_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 typename Base::GeometryModel GeometryModel;
 typedef typename Base::GeometryData GeometryData;

 // typedef std::vector<BroadPhaseManager,Eigen::aligned_allocator<BroadPhaseManager> >
 // BroadPhaseManagerVector;

 typedef std::vector<BroadPhaseManager> BroadPhaseManagerVector;

 BroadPhaseManagerPoolBase(
 const Model & model,
 const GeometryModel & geometry_model,
 const size_t pool_size = (size_t)omp_get_max_threads())
 : Base(model, geometry_model, pool_size)
 {
 init();
 }

 BroadPhaseManagerPoolBase(const BroadPhaseManagerPoolBase & other)
 : Base(other)
 , m_managers(other.m_managers)
 {
 }

 const BroadPhaseManager & getBroadPhaseManager(const size_t index) const
 {
 PINOCCHIO_CHECK_INPUT_ARGUMENT(
 index < m_managers.size(), "Index greater than the size of the manager vector.");
 return m_managers[index];
 }

 BroadPhaseManager & getBroadPhaseManager(const size_t index)
 {
 PINOCCHIO_CHECK_INPUT_ARGUMENT(
 index < m_managers.size(), "Index greater than the size of the manager vector.");
 return m_managers[index];
 }

 const BroadPhaseManagerVector & getBroadPhaseManagers() const
 {
 return m_managers;
 }

 BroadPhaseManagerVector & getBroadPhaseManagers()
 {
 return m_managers;
 }

 using Base::getData;
 using Base::getDatas;
 using Base::getGeometryData;
 using Base::getGeometryDatas;
 using Base::getGeometryModel;
 using Base::getGeometryModels;
 using Base::getModel;
 using Base::getModels;
 using Base::size;
 using Base::update;

 virtual void update(const GeometryData & geometry_data)
 {
 Base::update(geometry_data);

 for (size_t i = 0; i < size(); ++i)
 {
 m_managers[i].update(&getGeometryData(i));
 }
 }

 bool check() const
 {
 for (size_t i = 0; i < size(); ++i)
 {
 const BroadPhaseManager & manager = m_managers[i];
 bool res = true;
 res &= (&manager.getModel() == &getModel(i));
 res &= (&manager.getGeometryData() == &getGeometryData(i));

 res &= (&manager.getGeometryModel() == &getGeometryModel(i));
 res &= manager.check();

 if (!res)
 return false;
 }

 return true;
 }

 virtual ~BroadPhaseManagerPoolBase() {};

 protected:
 void init()
 {
 m_managers.reserve(size());
 for (size_t i = 0; i < size(); ++i)
 {
 m_managers.push_back(
 BroadPhaseManager(&getModel(i), &getGeometryModel(i), &getGeometryData(i)));
 }
 }

 BroadPhaseManagerVector m_managers;

 virtual void doResize(const size_t new_size)
 {
 m_managers.resize(new_size);
 if (size() < new_size)
 {
 typename BroadPhaseManagerVector::iterator it = m_managers.begin();
 std::advance(it, (long)(new_size - size()));
 std::fill(it, m_managers.end(), m_managers[0]);
 }
 }
 };
} // namespace pinocchio

#endif // ifndef __pinocchio_collision_pool_broadphase_manager_hpp__