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__