tree-broadphase-manager.hpp

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

#ifndef __pinocchio_collision_tree_broadphase_manager_hpp__
#define __pinocchio_collision_tree_broadphase_manager_hpp__

#include "pinocchio/collision/broadphase-manager.hpp"

namespace pinocchio
{

 template<typename _Manager>
 struct TreeBroadPhaseManagerTpl : public BroadPhaseManagerBase<TreeBroadPhaseManagerTpl<_Manager>>
 {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Manager Manager;
 typedef BroadPhaseManagerBase<TreeBroadPhaseManagerTpl<_Manager>> Base;
 typedef BroadPhaseManagerTpl<Manager> BroadPhaseManager;

 typedef std::vector<hpp::fcl::CollisionObject *> CollisionObjectPointerVector;
 typedef std::vector<BroadPhaseManager> BroadPhaseManagerVector;

 typedef typename BroadPhaseManager::Model Model;
 typedef typename BroadPhaseManager::GeometryModel GeometryModel;
 typedef typename BroadPhaseManager::GeometryData GeometryData;

 TreeBroadPhaseManagerTpl() // for std::vector
 : Base()
 {
 }

 TreeBroadPhaseManagerTpl(
 const Model * model_ptr,
 const GeometryModel * geometry_model_ptr,
 GeometryData * geometry_data_ptr)
 : Base(model_ptr, geometry_model_ptr, geometry_data_ptr)
 {
 init(static_cast<size_t>(model_ptr->njoints));
 }

 TreeBroadPhaseManagerTpl(const TreeBroadPhaseManagerTpl & other)
 : Base(other)
 {
 init(other.managers.size());
 }

 using Base::getGeometryData;
 using Base::getGeometryModel;
 using Base::getModel;

 void update(bool compute_local_aabb = false);

 void update(GeometryData * geom_data_ptr_new);

 ~TreeBroadPhaseManagerTpl()
 {
 }

 bool check() const;

 bool check(CollisionCallBackBase * callback) const;

 bool collide(CollisionObject & obj, CollisionCallBackBase * callback) const;

 bool collide(CollisionCallBackBase * callback) const;

 bool collide(TreeBroadPhaseManagerTpl & other_manager, CollisionCallBackBase * callback) const;

 // /// @brief Performs distance computation between one object and all the objects belonging to
 // the manager void distance(CollisionObject* obj, DistanceCallBackBase * callback) const;

 // /// @brief Performs distance test for the objects belonging to the manager (i.e., N^2 self
 // distance) void distance(DistanceCallBackBase * callback) const;

 // /// @brief Performs distance test with objects belonging to another manager
 // void distance(TreeBroadPhaseManagerTpl* other_manager, DistanceCallBackBase * callback)
 // const;

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

 BroadPhaseManagerVector & getBroadPhaseManagers()
 {
 return managers;
 }

 protected:
 BroadPhaseManagerVector managers;

 void init(const size_t njoints);

 }; // struct BroadPhaseManagerTpl<BroadPhaseManagerDerived>

} // namespace pinocchio

/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/collision/tree-broadphase-manager.hxx"

#endif // ifndef __pinocchio_collision_tree_broadphase_manager_hpp__