Program Listing for File tree-broadphase-manager.hpp

Return to documentation for file (include/pinocchio/collision/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__