5 #ifndef __pinocchio_algorithm_parallel_geometry_hpp__
6 #define __pinocchio_algorithm_parallel_geometry_hpp__
10 #include "pinocchio/multibody/pool/geometry.hpp"
11 #include "pinocchio/algorithm/geometry.hpp"
17 const GeometryModel & geom_model,
18 GeometryData & geom_data,
19 const bool stopAtFirstCollision =
false)
21 bool is_colliding =
false;
23 omp_set_num_threads(num_threads);
24 std::size_t cp_index = 0;
26 #pragma omp parallel for
27 for(cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index)
29 if(stopAtFirstCollision && is_colliding)
continue;
30 const CollisionPair & cp = geom_model.collisionPairs[cp_index];
32 if(geom_data.activeCollisionPairs[cp_index]
33 && !( geom_model.geometryObjects[cp.first].disableCollision
34 || geom_model.geometryObjects[cp.second].disableCollision))
36 fcl::CollisionRequest & collision_request = geom_data.collisionRequests[cp_index];
37 collision_request.distance_upper_bound = collision_request.security_margin + 1e-6;
39 fcl::CollisionResult & collision_result = geom_data.collisionResults[cp_index];
40 collision_result.clear();
42 fcl::Transform3f oM1 (toFclTransform3f(geom_data.oMg[cp.first ])),
43 oM2 (toFclTransform3f(geom_data.oMg[cp.second]));
45 const GeometryData::ComputeCollision & do_computations = geom_data.collision_functors[cp_index];
46 std::size_t res = do_computations(oM1, oM2, collision_request, collision_result);
48 if(!is_colliding && res)
51 geom_data.collisionPairIndex = cp_index;
59 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
61 const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
62 DataTpl<Scalar,Options,JointCollectionTpl> & data,
63 const GeometryModel & geom_model,
64 GeometryData & geom_data,
65 const Eigen::MatrixBase<ConfigVectorType> & q,
66 const bool stopAtFirstCollision =
false)
69 return computeCollisions(num_threads, geom_model, geom_data, stopAtFirstCollision);
72 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorPool,
typename CollisionVectorResult>
74 GeometryPoolTpl<Scalar,Options,JointCollectionTpl> & pool,
75 const Eigen::MatrixBase<ConfigVectorPool> & q,
76 const Eigen::MatrixBase<CollisionVectorResult> & res,
77 const bool stopAtFirstCollision =
false)
79 typedef GeometryPoolTpl<Scalar,Options,JointCollectionTpl> Pool;
80 typedef typename Pool::Model Model;
81 typedef typename Pool::Data Data;
82 typedef typename Pool::GeometryModel GeometryModel;
83 typedef typename Pool::GeometryData GeometryData;
84 typedef typename Pool::DataVector DataVector;
85 typedef typename Pool::GeometryDataVector GeometryDataVector;
87 const Model & model = pool.model();
88 const GeometryModel & geometry_model = pool.geometry_model();
89 DataVector & datas = pool.datas();
90 GeometryDataVector & geometry_datas = pool.geometry_datas();
91 CollisionVectorResult & res_ = res.const_cast_derived();
93 PINOCCHIO_CHECK_INPUT_ARGUMENT(num_threads <= pool.size(),
"The pool is too small");
94 PINOCCHIO_CHECK_ARGUMENT_SIZE(q.rows(), model.nq);
95 PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), res.size());
98 omp_set_num_threads(num_threads);
99 const Eigen::DenseIndex batch_size = res.size();
100 Eigen::DenseIndex i = 0;
102 #pragma omp parallel for
103 for(i = 0; i < batch_size; i++)
105 const int thread_id = omp_get_thread_num();
106 Data & data = datas[(size_t)thread_id];
107 GeometryData & geometry_data = geometry_datas[(size_t)thread_id];
108 res_[i] =
computeCollisions(model,data,geometry_model,geometry_data,q.col(i),stopAtFirstCollision);
113 #endif // ifndef __pinocchio_algorithm_parallel_geometry_hpp__