5 #ifndef __pinocchio_algorithm_parallel_geometry_hpp__
6 #define __pinocchio_algorithm_parallel_geometry_hpp__
19 const bool stopAtFirstCollision =
false)
21 bool is_colliding =
false;
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;
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();
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)
59 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
65 const Eigen::MatrixBase<ConfigVectorType> &
q,
66 const bool stopAtFirstCollision =
false)
72 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorPool,
typename CollisionVectorResult>
75 const Eigen::MatrixBase<ConfigVectorPool> &
q,
76 const Eigen::MatrixBase<CollisionVectorResult> & res,
77 const bool stopAtFirstCollision =
false)
84 typedef typename Pool::DataVector DataVector;
85 typedef typename Pool::GeometryDataVector GeometryDataVector;
89 DataVector & datas =
pool.datas();
90 GeometryDataVector & geometry_datas =
pool.geometry_datas();
91 CollisionVectorResult & res_ =
res.const_cast_derived();
100 Eigen::DenseIndex
i = 0;
102 #pragma omp parallel for
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];
113 #endif // ifndef __pinocchio_algorithm_parallel_geometry_hpp__