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__