5 #ifndef __pinocchio_collision_parallel_geometry_hpp__
6 #define __pinocchio_collision_parallel_geometry_hpp__
22 const bool stopAtFirstCollision =
false)
24 bool is_colliding =
false;
34 #pragma omp parallel for schedule(dynamic)
35 for (omp_cp_index = 0; omp_cp_index <
batch_size; ++omp_cp_index)
37 size_t cp_index =
static_cast<size_t>(omp_cp_index);
39 if (stopAtFirstCollision && is_colliding)
51 if (!is_colliding &&
res)
65 template<
typename,
int>
66 class JointCollectionTpl,
67 typename ConfigVectorType>
74 const Eigen::MatrixBase<ConfigVectorType> &
q,
75 const bool stopAtFirstCollision =
false)
84 template<
typename,
int>
85 class JointCollectionTpl,
86 typename ConfigVectorPool,
87 typename CollisionVectorResult>
91 const Eigen::MatrixBase<ConfigVectorPool> &
q,
92 const Eigen::MatrixBase<CollisionVectorResult> &
res,
93 const bool stopAtFirstCollisionInConfiguration =
false,
94 const bool stopAtFirstCollisionInBatch =
false)
101 typedef typename Pool::ModelVector ModelVector;
102 typedef typename Pool::DataVector DataVector;
103 typedef typename Pool::GeometryModelVector GeometryModelVector;
104 typedef typename Pool::GeometryDataVector GeometryDataVector;
109 const ModelVector & models =
pool.getModels();
110 const Model & model_check = models[0];
111 const GeometryModelVector & geometry_models =
pool.getGeometryModels();
112 DataVector & datas =
pool.getDatas();
113 GeometryDataVector & geometry_datas =
pool.getGeometryDatas();
114 CollisionVectorResult & res_ =
res.const_cast_derived();
123 if (stopAtFirstCollisionInBatch)
125 bool is_colliding =
false;
126 Eigen::DenseIndex
i = 0;
127 #pragma omp parallel for schedule(static)
133 const int thread_id = omp_get_thread_num();
135 const Model &
model = models[(size_t)thread_id];
136 Data &
data = datas[(size_t)thread_id];
137 const GeometryModel & geometry_model = geometry_models[(size_t)thread_id];
138 GeometryData & geometry_data = geometry_datas[(size_t)thread_id];
140 model,
data, geometry_model, geometry_data,
q.col(
i),
141 stopAtFirstCollisionInConfiguration);
151 Eigen::DenseIndex
i = 0;
152 #pragma omp parallel for schedule(static)
155 const int thread_id = omp_get_thread_num();
157 const Model &
model = models[(size_t)thread_id];
158 Data &
data = datas[(size_t)thread_id];
159 const GeometryModel & geometry_model = geometry_models[(size_t)thread_id];
160 GeometryData & geometry_data = geometry_datas[(size_t)thread_id];
162 model,
data, geometry_model, geometry_data,
q.col(
i),
163 stopAtFirstCollisionInConfiguration);
169 #endif // ifndef __pinocchio_collision_parallel_geometry_hpp__