5 #ifndef __pinocchio_collision_parallel_broadphase_hpp__
6 #define __pinocchio_collision_parallel_broadphase_hpp__
19 typename BroadPhaseManagerDerived,
22 template<
typename,
int>
23 class JointCollectionTpl,
24 typename ConfigVectorPool,
25 typename CollisionVectorResult>
29 const Eigen::MatrixBase<ConfigVectorPool> &
q,
30 const Eigen::MatrixBase<CollisionVectorResult> &
res,
31 const bool stopAtFirstCollisionInConfiguration =
false,
32 const bool stopAtFirstCollisionInBatch =
false)
38 typedef typename Pool::ModelVector ModelVector;
39 typedef typename Pool::DataVector DataVector;
40 typedef typename Pool::BroadPhaseManager BroadPhaseManager;
41 typedef typename Pool::BroadPhaseManagerVector BroadPhaseManagerVector;
43 const ModelVector & models =
pool.getModels();
44 const Model & model_check = models[0];
45 DataVector & datas =
pool.getDatas();
46 BroadPhaseManagerVector & broadphase_managers =
pool.getBroadPhaseManagers();
47 CollisionVectorResult & res_ =
res.const_cast_derived();
57 if (stopAtFirstCollisionInBatch)
59 bool is_colliding =
false;
60 Eigen::DenseIndex
i = 0;
61 #pragma omp parallel for schedule(static)
67 const int thread_id = omp_get_thread_num();
68 const Model &
model = models[(size_t)thread_id];
69 Data &
data = datas[(size_t)thread_id];
70 BroadPhaseManager & manager = broadphase_managers[(size_t)thread_id];
82 Eigen::DenseIndex
i = 0;
83 #pragma omp parallel for schedule(static)
86 const int thread_id = omp_get_thread_num();
87 const Model &
model = models[(size_t)thread_id];
88 Data &
data = datas[(size_t)thread_id];
89 BroadPhaseManager & manager = broadphase_managers[(size_t)thread_id];
101 typename BroadPhaseManagerDerived,
104 template<
typename,
int>
105 class JointCollectionTpl>
109 const std::vector<Eigen::MatrixXd> & trajectories,
110 std::vector<VectorXb> &
res,
111 const bool stopAtFirstCollisionInTrajectory =
false)
117 typedef typename Pool::ModelVector ModelVector;
118 typedef typename Pool::DataVector DataVector;
119 typedef typename Pool::BroadPhaseManager BroadPhaseManager;
120 typedef typename Pool::BroadPhaseManagerVector BroadPhaseManagerVector;
122 const ModelVector & models =
pool.getModels();
123 const Model & model_check = models[0];
124 DataVector & datas =
pool.getDatas();
125 BroadPhaseManagerVector & broadphase_managers =
pool.getBroadPhaseManagers();
130 for (
size_t k = 0; k < trajectories.size(); ++k)
145 #pragma omp parallel for schedule(static)
148 size_t i =
static_cast<size_t>(omp_i);
149 const int thread_id = omp_get_thread_num();
150 const Model &
model = models[size_t(thread_id)];
151 Data &
data = datas[(size_t)thread_id];
152 const Eigen::MatrixXd & current_traj = trajectories[
i];
154 res_current_traj.fill(
false);
155 BroadPhaseManager & manager = broadphase_managers[size_t(thread_id)];
157 for (Eigen::DenseIndex col_id = 0; col_id < current_traj.cols(); ++col_id)
159 res_current_traj[col_id] =
161 if (res_current_traj[col_id] && stopAtFirstCollisionInTrajectory)
168 #endif // ifndef __pinocchio_collision_parallel_broadphase_hpp__