Program Listing for File broadphase.hpp
↰ Return to documentation for file (include/pinocchio/collision/broadphase.hpp
)
//
// Copyright (c) 2022 INRIA
//
#ifndef __pinocchio_collision_broadphase_hpp__
#define __pinocchio_collision_broadphase_hpp__
#include <hpp/fcl/broadphase/broadphase_collision_manager.h>
#include "pinocchio/multibody/fcl.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/collision/broadphase-manager.hpp"
#include "pinocchio/collision/broadphase-callbacks.hpp"
namespace pinocchio
{
template<typename BroadPhaseManagerDerived>
bool computeCollisions(
BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
CollisionCallBackBase * callback)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(broadphase_manager.check(callback));
broadphase_manager.collide(callback);
callback->done();
return callback->collision;
}
template<typename BroadPhaseManagerDerived>
bool computeCollisions(
BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
const bool stopAtFirstCollision = false)
{
CollisionCallBackDefault callback(
broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(),
stopAtFirstCollision);
return computeCollisions(broadphase_manager, &callback);
}
template<
typename Scalar,
int Options,
template<typename, int> class JointCollectionTpl,
typename BroadPhaseManagerDerived,
typename ConfigVectorType>
inline bool computeCollisions(
const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
DataTpl<Scalar, Options, JointCollectionTpl> & data,
BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
CollisionCallBackBase * callback,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
updateGeometryPlacements(
model, data, broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(), q);
broadphase_manager.update(false);
return computeCollisions(broadphase_manager, &callback);
}
template<
typename Scalar,
int Options,
template<typename, int> class JointCollectionTpl,
typename BroadPhaseManagerDerived,
typename ConfigVectorType>
inline bool computeCollisions(
const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
DataTpl<Scalar, Options, JointCollectionTpl> & data,
BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
const Eigen::MatrixBase<ConfigVectorType> & q,
const bool stopAtFirstCollision = false)
{
updateGeometryPlacements(
model, data, broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(), q);
broadphase_manager.update(false);
CollisionCallBackDefault callback(
broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(),
stopAtFirstCollision);
return computeCollisions(broadphase_manager, &callback);
}
} // namespace pinocchio
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/collision/broadphase.hxx"
#endif // ifndef __pinocchio_collision_broadphase_hpp__