Program Listing for File broadphase-manager.hpp

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