broadphase-manager.hpp

//
// Copyright (c) 2022 INRIA
//
 
#ifndef __pinocchio_collision_broadphase_manager_hpp__
#define __pinocchio_collision_broadphase_manager_hpp__
 
#include <hpp/fcl/broadphase/broadphase_collision_manager.h>
 
#include "pinocchio/collision/broadphase-manager-base.hpp"
#include "pinocchio/multibody/geometry-object-filter.hpp"
 
namespace pinocchio
{
 
  template<typename _Manager>
  struct BroadPhaseManagerTpl : public BroadPhaseManagerBase<BroadPhaseManagerTpl<_Manager>>
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    typedef BroadPhaseManagerBase<BroadPhaseManagerTpl<_Manager>> Base;
    typedef std::vector<CollisionObject> CollisionObjectVector;
    typedef Eigen::VectorXd VectorXs;
    typedef _Manager Manager;
 
    typedef ::pinocchio::Model Model;
    typedef ::pinocchio::GeometryModel GeometryModel;
    typedef ::pinocchio::GeometryData GeometryData;
 
    BroadPhaseManagerTpl() // for std::vector
    : Base()
    {
    }
 
    BroadPhaseManagerTpl(
      const Model * model_ptr,
      const GeometryModel * geometry_model_ptr,
      GeometryData * geometry_data_ptr,
      const GeometryObjectFilterBase & filter = GeometryObjectFilterNothing())
    : Base(model_ptr, geometry_model_ptr, geometry_data_ptr)
    {
      const GeometryModel & geom_model = *geometry_model_ptr;
      selected_geometry_objects = filter.apply(geom_model.geometryObjects);
 
      geometry_to_collision_index.resize(
        geometry_model_ptr->geometryObjects.size(), (std::numeric_limits<size_t>::max)());
      collision_object_is_active.resize(selected_geometry_objects.size(), true);
      for (size_t k = 0; k < selected_geometry_objects.size(); ++k)
      {
        const size_t geometry_id = selected_geometry_objects[k];
        geometry_to_collision_index[geometry_id] = k;
        collision_object_is_active[k] = !geom_model.geometryObjects[geometry_id].disableCollision;
      }
 
      selected_collision_pairs.reserve(geom_model.collisionPairs.size());
      for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
      {
        const CollisionPair & pair = geom_model.collisionPairs[k];
        if (
          geometry_to_collision_index[pair.first] != (std::numeric_limits<size_t>::max)()
          && geometry_to_collision_index[pair.second] != (std::numeric_limits<size_t>::max)())
        {
          selected_collision_pairs.push_back(k);
        }
 
        selected_collision_pairs.resize(selected_collision_pairs.size());
      }
 
      collision_object_inflation.resize(
        static_cast<Eigen::DenseIndex>(selected_geometry_objects.size()));
 
      init();
    }
 
    BroadPhaseManagerTpl(const BroadPhaseManagerTpl & other)
    : Base(other)
    , collision_object_inflation(other.collision_object_inflation.size())
    , selected_geometry_objects(other.selected_geometry_objects)
    , geometry_to_collision_index(other.geometry_to_collision_index)
    , selected_collision_pairs(other.selected_collision_pairs)
    , collision_object_is_active(other.collision_object_is_active)
    {
      init();
    }
 
    using Base::getGeometryData;
    using Base::getGeometryModel;
    using Base::getModel;
 
    void update(bool compute_local_aabb = false);
 
    void update(GeometryData * geom_data_ptr_new);
 
    ~BroadPhaseManagerTpl();
 
    bool check() const;
 
    bool check(CollisionCallBackBase * callback) const;
 
    const CollisionObjectVector & getCollisionObjects() const
    {
      return collision_objects;
    }
    CollisionObjectVector & getCollisionObjects()
    {
      return collision_objects;
    }
 
    const VectorXs & getCollisionObjectInflation()
    {
      return collision_object_inflation;
    }
 
    bool collide(CollisionObject & obj, CollisionCallBackBase * callback) const;
 
    bool collide(CollisionCallBackBase * callback) const;
 
    bool collide(BroadPhaseManagerTpl & 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(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase * callback)
    //  const;
 
    Manager & getManager()
    {
      return manager;
    }
 
    const Manager & getManager() const
    {
      return manager;
    }
 
    const std::vector<bool> & getCollisionObjectStatus() const
    {
      return collision_object_is_active;
    }
 
  protected:
    Manager manager;
 
    CollisionObjectVector collision_objects;
 
    VectorXs collision_object_inflation;
 
    std::vector<size_t> selected_geometry_objects;
 
    std::vector<size_t> geometry_to_collision_index;
 
    std::vector<size_t> selected_collision_pairs;
 
    std::vector<bool> collision_object_is_active;
 
    void init();
 
  }; // struct BroadPhaseManagerTpl<BroadPhaseManagerDerived>
 
} // namespace pinocchio
 
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/collision/broadphase-manager.hxx"
 
#endif // ifndef __pinocchio_collision_broadphase_manager_hpp__