GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/geometry.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 112 250 44.8%
Branches: 118 556 21.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2022 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_multibody_geometry_hxx__
6 #define __pinocchio_multibody_geometry_hxx__
7
8 #include <algorithm>
9
10 #include "pinocchio/multibody/model.hpp"
11
12 #if BOOST_VERSION / 100 % 1000 >= 60
13 #include <boost/bind/bind.hpp>
14 #include <boost/utility.hpp>
15 #else
16 #include <boost/bind.hpp>
17 #endif
18 #include <boost/foreach.hpp>
19
20 #include <sstream>
21 /// @cond DEV
22
23 namespace pinocchio
24 {
25
26 ✗ inline CollisionPair::CollisionPair()
27 ✗ : Base((std::numeric_limits<GeomIndex>::max)(), (std::numeric_limits<GeomIndex>::max)())
28 {
29 }
30
31 ///
32 /// \brief Default constructor of a collision pair from two collision object indexes.
33 /// \remarks The two indexes must be different, otherwise the constructor throws.
34 ///
35 /// \param[in] co1 Index of the first collision object.
36 /// \param[in] co2 Index of the second collision object.
37 ///
38 191 inline CollisionPair::CollisionPair(const GeomIndex co1, const GeomIndex co2)
39 191 : Base(co1, co2)
40 {
41
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 191 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 191 PINOCCHIO_CHECK_INPUT_ARGUMENT(co1 != co2, "The index of collision objects must not be equal.");
42 191 }
43
44 ✗ inline bool CollisionPair::operator==(const CollisionPair & rhs) const
45 {
46 ✗ return (first == rhs.first && second == rhs.second)
47 ✗ || (first == rhs.second && second == rhs.first);
48 }
49
50 ✗ inline bool CollisionPair::operator!=(const CollisionPair & other) const
51 {
52 ✗ return !(*this == other);
53 }
54
55 ✗ inline void CollisionPair::disp(std::ostream & os) const
56 {
57 ✗ os << "collision pair (" << first << "," << second << ")\n";
58 }
59
60 ✗ inline std::ostream & operator<<(std::ostream & os, const CollisionPair & X)
61 {
62 ✗ X.disp(os);
63 ✗ return os;
64 }
65
66 ✗ inline GeometryModel GeometryModel::clone() const
67 {
68 ✗ GeometryModel res;
69 ✗ res.ngeoms = ngeoms;
70 ✗ res.collisionPairs = collisionPairs;
71 ✗ res.collisionPairMapping = collisionPairMapping;
72
73 ✗ res.geometryObjects.reserve(geometryObjects.size());
74 ✗ for (const GeometryObject & geometry_object : geometryObjects)
75 {
76 ✗ res.geometryObjects.push_back(geometry_object.clone());
77 }
78
79 ✗ return res;
80 }
81
82 13 inline GeometryData::GeometryData(const GeometryModel & geom_model)
83
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 13 : oMg(geom_model.ngeoms)
84
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 , activeCollisionPairs(geom_model.collisionPairs.size(), true)
85 #ifdef PINOCCHIO_WITH_HPP_FCL
86
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 13 , distanceRequests(geom_model.collisionPairs.size(), hpp::fcl::DistanceRequest(true))
87
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 , distanceResults(geom_model.collisionPairs.size())
88
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 26 , collisionRequests(
89
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 geom_model.collisionPairs.size(), hpp::fcl::CollisionRequest(::hpp::fcl::NO_REQUEST, 1))
90
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 , collisionResults(geom_model.collisionPairs.size())
91 13 , radius()
92 13 , collisionPairIndex(0)
93 #endif // PINOCCHIO_WITH_HPP_FCL
94 13 , innerObjects()
95
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 26 , outerObjects()
96 {
97 #ifdef PINOCCHIO_WITH_HPP_FCL
98
18/30
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 70 times.
✓ Branch 25 taken 70 times.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 70 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 83 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 83 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 70 times.
✓ Branch 37 taken 13 times.
✓ Branch 38 taken 70 times.
✓ Branch 39 taken 13 times.
 153 BOOST_FOREACH (hpp::fcl::CollisionRequest & creq, collisionRequests)
99 {
100 70 creq.enable_cached_gjk_guess = true;
101 }
102 #if HPP_FCL_VERSION_AT_LEAST(1, 4, 5)
103
18/30
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 70 times.
✓ Branch 25 taken 70 times.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 70 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 83 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 83 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 70 times.
✓ Branch 37 taken 13 times.
✓ Branch 38 taken 70 times.
✓ Branch 39 taken 13 times.
 153 BOOST_FOREACH (hpp::fcl::DistanceRequest & dreq, distanceRequests)
104 {
105 70 dreq.enable_cached_gjk_guess = true;
106 }
107 #endif
108
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 13 collision_functors.reserve(geom_model.collisionPairs.size());
109
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 13 distance_functors.reserve(geom_model.collisionPairs.size());
110
111
2/2
✓ Branch 1 taken 70 times.
✓ Branch 2 taken 13 times.
 83 for (size_t cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index)
112 {
113 70 const CollisionPair & cp = geom_model.collisionPairs[cp_index];
114 70 const GeometryObject & obj_1 = geom_model.geometryObjects[cp.first];
115 70 const GeometryObject & obj_2 = geom_model.geometryObjects[cp.second];
116
117
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 collision_functors.push_back(ComputeCollision(obj_1, obj_2));
118
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 distance_functors.push_back(ComputeDistance(obj_1, obj_2));
119 }
120 #endif
121
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 fillInnerOuterObjectMaps(geom_model);
122 13 }
123
124 12 inline GeometryData::GeometryData(const GeometryData & other)
125 12 : oMg(other.oMg)
126
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 , activeCollisionPairs(other.activeCollisionPairs)
127 #ifdef PINOCCHIO_WITH_HPP_FCL
128
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 , distanceRequests(other.distanceRequests)
129
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 , distanceResults(other.distanceResults)
130
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 , collisionRequests(other.collisionRequests)
131
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 , collisionResults(other.collisionResults)
132
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 , radius(other.radius)
133 12 , collisionPairIndex(other.collisionPairIndex)
134
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 , collision_functors(other.collision_functors)
135
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 , distance_functors(other.distance_functors)
136 #endif // PINOCCHIO_WITH_HPP_FCL
137
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 , innerObjects(other.innerObjects)
138
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 , outerObjects(other.outerObjects)
139 {
140 12 }
141
142 ✗ inline GeometryData & GeometryData::operator=(const GeometryData & other)
143 {
144 ✗ if (this != &other)
145 {
146 ✗ oMg = other.oMg;
147 ✗ activeCollisionPairs = other.activeCollisionPairs;
148 #ifdef PINOCCHIO_WITH_HPP_FCL
149 ✗ distanceRequests = other.distanceRequests;
150 ✗ distanceResults = other.distanceResults;
151 ✗ collisionRequests = other.collisionRequests;
152 ✗ collisionResults = other.collisionResults;
153 ✗ radius = other.radius;
154 ✗ collisionPairIndex = other.collisionPairIndex;
155 ✗ collision_functors = other.collision_functors;
156 ✗ distance_functors = other.distance_functors;
157 #endif // PINOCCHIO_WITH_HPP_FCL
158 ✗ innerObjects = other.innerObjects;
159 ✗ outerObjects = other.outerObjects;
160 }
161 ✗ return *this;
162 }
163
164 25 inline GeometryData::~GeometryData()
165 {
166 25 }
167
168 template<typename S2, int O2, template<typename, int> class JointCollectionTpl>
169 ✗ GeomIndex GeometryModel::addGeometryObject(
170 const GeometryObject & object, const ModelTpl<S2, O2, JointCollectionTpl> & model)
171 {
172 ✗ if (object.parentFrame < (FrameIndex)model.nframes)
173 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(
174 model.frames[object.parentFrame].parentJoint == object.parentJoint,
175 "The object joint parent and its frame joint parent do not match.");
176
177 ✗ Eigen::DenseIndex idx = (Eigen::DenseIndex)(ngeoms++);
178 ✗ geometryObjects.push_back(object);
179 ✗ geometryObjects.back().parentJoint = model.frames[object.parentFrame].parentJoint;
180
181 ✗ collisionPairMapping.conservativeResize(idx + 1, idx + 1);
182 ✗ collisionPairMapping.col(idx).fill(-1);
183 ✗ collisionPairMapping.row(idx).fill(-1);
184 ✗ assert(collisionPairMapping.cols() == (Eigen::DenseIndex)ngeoms);
185
186 ✗ return (GeomIndex)idx;
187 }
188
189 905 inline GeomIndex GeometryModel::addGeometryObject(const GeometryObject & object)
190 {
191 905 Eigen::DenseIndex idx = (Eigen::DenseIndex)(ngeoms++);
192 905 geometryObjects.push_back(object);
193
194 905 collisionPairMapping.conservativeResize(idx + 1, idx + 1);
195
1/2
✓ Branch 2 taken 905 times.
✗ Branch 3 not taken.
 905 collisionPairMapping.col(idx).fill(-1);
196
1/2
✓ Branch 2 taken 905 times.
✗ Branch 3 not taken.
 905 collisionPairMapping.row(idx).fill(-1);
197
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 905 times.
 905 assert(collisionPairMapping.cols() == (Eigen::DenseIndex)ngeoms);
198
199 905 return (GeomIndex)idx;
200 }
201
202 ✗ inline void GeometryModel::removeGeometryObject(const std::string & name)
203 {
204 ✗ GeomIndex i = 0;
205 ✗ GeometryObjectVector::iterator it;
206 ✗ for (it = geometryObjects.begin(); it != geometryObjects.end(); ++it, ++i)
207 {
208 ✗ if (it->name == name)
209 {
210 ✗ break;
211 }
212 }
213 ✗ PINOCCHIO_THROW(
214 it != geometryObjects.end(), std::invalid_argument,
215 (std::string("Object ") + name + std::string(" does not belong to model")).c_str());
216 // Remove all collision pairs that contain i as first or second index,
217 ✗ for (CollisionPairVector::iterator itCol = collisionPairs.begin();
218 ✗ itCol != collisionPairs.end(); ++itCol)
219 {
220 ✗ if ((itCol->first == i) || (itCol->second == i))
221 {
222 ✗ itCol = collisionPairs.erase(itCol);
223 ✗ itCol--;
224 }
225 else
226 {
227 // Indices of objects after the one that is removed should be decreased by one.
228 ✗ if (itCol->first > i)
229 ✗ itCol->first--;
230 ✗ if (itCol->second > i)
231 ✗ itCol->second--;
232 }
233 }
234 ✗ geometryObjects.erase(it);
235 ✗ ngeoms--;
236 }
237
238 1 inline GeomIndex GeometryModel::getGeometryId(const std::string & name) const
239 {
240 #if BOOST_VERSION / 100 % 1000 >= 60
241 using namespace boost::placeholders;
242 #endif
243
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 GeometryObjectVector::const_iterator it = std::find_if(
244 geometryObjects.begin(), geometryObjects.end(),
245
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 boost::bind(&GeometryObject::name, _1) == name);
246 1 return GeomIndex(it - geometryObjects.begin());
247 }
248
249 ✗ inline bool GeometryModel::existGeometryName(const std::string & name) const
250 {
251 #if BOOST_VERSION / 100 % 1000 >= 60
252 using namespace boost::placeholders;
253 #endif
254 ✗ return std::find_if(
255 geometryObjects.begin(), geometryObjects.end(),
256 ✗ boost::bind(&GeometryObject::name, _1) == name)
257 ✗ != geometryObjects.end();
258 }
259
260 13 inline void GeometryData::fillInnerOuterObjectMaps(const GeometryModel & geomModel)
261 {
262 13 innerObjects.clear();
263 13 outerObjects.clear();
264
265
2/2
✓ Branch 1 taken 283 times.
✓ Branch 2 taken 13 times.
 296 for (GeomIndex gid = 0; gid < geomModel.geometryObjects.size(); gid++)
266
2/4
✓ Branch 2 taken 283 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 283 times.
✗ Branch 6 not taken.
 283 innerObjects[geomModel.geometryObjects[gid].parentJoint].push_back(gid);
267
268
18/30
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 70 times.
✓ Branch 25 taken 70 times.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 70 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 83 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 83 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 70 times.
✓ Branch 37 taken 13 times.
✓ Branch 38 taken 70 times.
✓ Branch 39 taken 13 times.
 153 BOOST_FOREACH (const CollisionPair & pair, geomModel.collisionPairs)
269 {
270
2/4
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
 70 outerObjects[geomModel.geometryObjects[pair.first].parentJoint].push_back(pair.second);
271 }
272 13 }
273
274 ✗ inline std::ostream & operator<<(std::ostream & os, const GeometryModel & geomModel)
275 {
276 ✗ os << "Nb geometry objects = " << geomModel.ngeoms << std::endl;
277
278 ✗ for (GeomIndex i = 0; i < (GeomIndex)(geomModel.ngeoms); ++i)
279 {
280 ✗ os << geomModel.geometryObjects[i] << std::endl;
281 }
282
283 ✗ return os;
284 }
285
286 ✗ inline std::ostream & operator<<(std::ostream & os, const GeometryData & geomData)
287 {
288 #ifdef PINOCCHIO_WITH_HPP_FCL
289 ✗ os << "Number of collision pairs = " << geomData.activeCollisionPairs.size() << std::endl;
290
291 ✗ for (PairIndex i = 0; i < (PairIndex)(geomData.activeCollisionPairs.size()); ++i)
292 {
293 ✗ os << "Pairs " << i << (geomData.activeCollisionPairs[i] ? " active" : " inactive")
294 ✗ << std::endl;
295 }
296 #else
297 os << "WARNING** Without fcl library, no collision checking or distance computations are "
298 "possible. Only geometry placements can be computed."
299 << std::endl;
300 #endif
301 ✗ os << "Number of geometry objects = " << geomData.oMg.size() << std::endl;
302
303 ✗ return os;
304 }
305
306 191 inline void GeometryModel::addCollisionPair(const CollisionPair & pair)
307 {
308
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 191 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 191 PINOCCHIO_CHECK_INPUT_ARGUMENT(
309 pair.first < ngeoms, "The input pair.first is larger than the number of geometries "
310 "contained in the GeometryModel");
311
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 191 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 191 PINOCCHIO_CHECK_INPUT_ARGUMENT(
312 pair.second < ngeoms, "The input pair.second is larger than the number of geometries "
313 "contained in the GeometryModel");
314
1/2
✓ Branch 1 taken 191 times.
✗ Branch 2 not taken.
 191 if (!existCollisionPair(pair))
315 {
316 191 collisionPairs.push_back(pair);
317
318 382 collisionPairMapping((Eigen::DenseIndex)pair.second, (Eigen::DenseIndex)pair.first) =
319 191 (int)(collisionPairs.size() - 1);
320 191 collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second) =
321 191 collisionPairMapping(
322 191 (Eigen::DenseIndex)pair.second,
323 191 (Eigen::DenseIndex)pair.first); // make symmetric
324 }
325 191 }
326
327 ✗ inline void GeometryModel::setCollisionPairs(const MatrixXb & map, const bool upper)
328 {
329 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(
330 map.rows(), (Eigen::DenseIndex)ngeoms, "Input map does not have the correct number of rows.");
331 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(
332 map.cols(), (Eigen::DenseIndex)ngeoms,
333 "Input map does not have the correct number of columns.");
334 ✗ removeAllCollisionPairs();
335
336 ✗ for (Eigen::DenseIndex i = 0; i < (Eigen::DenseIndex)ngeoms; ++i)
337 {
338 ✗ for (Eigen::DenseIndex j = i + 1; j < (Eigen::DenseIndex)ngeoms; ++j)
339 {
340 ✗ if (upper)
341 {
342 ✗ if (map(i, j))
343 ✗ addCollisionPair(CollisionPair((std::size_t)i, (std::size_t)j));
344 }
345 else
346 {
347 ✗ if (map(j, i))
348 ✗ addCollisionPair(CollisionPair((std::size_t)i, (std::size_t)j));
349 }
350 }
351 }
352 }
353
354 1 inline void GeometryModel::addAllCollisionPairs()
355 {
356 1 removeAllCollisionPairs();
357
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 1 times.
 21 for (GeomIndex i = 0; i < ngeoms; ++i)
358 {
359 20 const JointIndex joint_i = geometryObjects[i].parentJoint;
360
2/2
✓ Branch 0 taken 190 times.
✓ Branch 1 taken 20 times.
 210 for (GeomIndex j = i + 1; j < ngeoms; ++j)
361 {
362 190 const JointIndex joint_j = geometryObjects[j].parentJoint;
363
1/2
✓ Branch 0 taken 190 times.
✗ Branch 1 not taken.
 190 if (joint_i != joint_j)
364
1/2
✓ Branch 2 taken 190 times.
✗ Branch 3 not taken.
 190 addCollisionPair(CollisionPair(i, j));
365 }
366 }
367 1 }
368
369 120 inline void GeometryModel::removeCollisionPair(const CollisionPair & pair)
370 {
371
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 120 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 120 PINOCCHIO_CHECK_INPUT_ARGUMENT(
372 pair.first < ngeoms, "The input pair.first is larger than the number of geometries "
373 "contained in the GeometryModel");
374
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 120 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 120 PINOCCHIO_CHECK_INPUT_ARGUMENT(
375 pair.second < ngeoms, "The input pair.second is larger than the number of geometries "
376 "contained in the GeometryModel");
377
378 120 const long index = (long)findCollisionPair(pair);
379
380
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 if (index != (long)collisionPairs.size())
381 {
382 240 collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second) =
383 120 collisionPairMapping((Eigen::DenseIndex)pair.second, (Eigen::DenseIndex)pair.first) = -1;
384
1/2
✓ Branch 4 taken 120 times.
✗ Branch 5 not taken.
 120 collisionPairs.erase(collisionPairs.begin() + index);
385
386
2/2
✓ Branch 1 taken 2400 times.
✓ Branch 2 taken 120 times.
 2520 for (Eigen::DenseIndex i = 0; i < collisionPairMapping.cols(); ++i)
387 {
388
2/2
✓ Branch 1 taken 22800 times.
✓ Branch 2 taken 2400 times.
 25200 for (Eigen::DenseIndex j = i + 1; j < collisionPairMapping.cols(); ++j)
389 {
390
2/2
✓ Branch 1 taken 8328 times.
✓ Branch 2 taken 14472 times.
 22800 if (collisionPairMapping(i, j) > index)
391 {
392 8328 collisionPairMapping(i, j)--;
393 8328 collisionPairMapping(j, i) = collisionPairMapping(i, j);
394 }
395 }
396 }
397 }
398 120 }
399
400 1 inline void GeometryModel::removeAllCollisionPairs()
401 {
402 1 collisionPairs.clear();
403
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 collisionPairMapping.fill(-1);
404 1 }
405
406 191 inline bool GeometryModel::existCollisionPair(const CollisionPair & pair) const
407 {
408 191 return collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second)
409 191 != -1;
410 }
411
412 15780 inline PairIndex GeometryModel::findCollisionPair(const CollisionPair & pair) const
413 {
414 15780 int res = collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second);
415
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15780 times.
 15780 if (res == -1)
416 ✗ return collisionPairs.size();
417 else
418 15780 return (PairIndex)res;
419 }
420
421 ✗ inline void GeometryData::activateCollisionPair(const PairIndex pair_id)
422 {
423 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(
424 pair_id < activeCollisionPairs.size(),
425 "The input argument pair_id is larger than the number of "
426 "collision pairs contained in activeCollisionPairs.");
427 ✗ activeCollisionPairs[pair_id] = true;
428 }
429
430 ✗ inline void GeometryData::activateAllCollisionPairs()
431 {
432 ✗ std::fill(activeCollisionPairs.begin(), activeCollisionPairs.end(), true);
433 }
434
435 ✗ inline void GeometryData::setActiveCollisionPairs(
436 const GeometryModel & geom_model, const MatrixXb & map, const bool upper)
437 {
438 ✗ const Eigen::DenseIndex ngeoms = (Eigen::DenseIndex)geom_model.ngeoms;
439 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(
440 map.rows(), ngeoms, "Input map does not have the correct number of rows.");
441 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(
442 map.cols(), ngeoms, "Input map does not have the correct number of columns.");
443 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(
444 geom_model.collisionPairs.size(), activeCollisionPairs.size(),
445 "Current geometry data and the input geometry model are not conistent.");
446
447 ✗ for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
448 {
449 ✗ const CollisionPair & cp = geom_model.collisionPairs[k];
450
451 Eigen::DenseIndex i, j;
452 ✗ if (upper)
453 {
454 ✗ j = (Eigen::DenseIndex)std::max(cp.first, cp.second);
455 ✗ i = (Eigen::DenseIndex)std::min(cp.first, cp.second);
456 }
457 else
458 {
459 ✗ i = (Eigen::DenseIndex)std::max(cp.first, cp.second);
460 ✗ j = (Eigen::DenseIndex)std::min(cp.first, cp.second);
461 }
462
463 ✗ activeCollisionPairs[k] = map(i, j);
464 }
465 }
466
467 ✗ inline void GeometryData::setGeometryCollisionStatus(
468 const GeometryModel & geom_model, const GeomIndex geom_id, const bool enable_collision)
469 {
470 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(
471 geom_id < geom_model.ngeoms, "The index of the geometry is not valid");
472 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(
473 geom_model.collisionPairs.size(), activeCollisionPairs.size(),
474 "Current geometry data and the input geometry model are not conistent.");
475
476 ✗ for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
477 {
478 ✗ const CollisionPair & cp = geom_model.collisionPairs[k];
479 ✗ if (cp.first == geom_id || cp.second == geom_id)
480 {
481 ✗ activeCollisionPairs[k] = enable_collision;
482 }
483 }
484 }
485
486 #ifdef PINOCCHIO_WITH_HPP_FCL
487 ✗ inline void GeometryData::setSecurityMargins(
488 const GeometryModel & geom_model,
489 const MatrixXs & security_margin_map,
490 const bool upper,
491 const bool sync_distance_upper_bound)
492 {
493 ✗ const Eigen::DenseIndex ngeoms = (Eigen::DenseIndex)geom_model.ngeoms;
494 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(
495 security_margin_map.rows(), ngeoms, "Input map does not have the correct number of rows.");
496 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(
497 security_margin_map.cols(), ngeoms, "Input map does not have the correct number of columns.");
498 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(
499 geom_model.collisionPairs.size(), collisionRequests.size(),
500 "Current geometry data and the input geometry model are not consistent.");
501
502 ✗ for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
503 {
504 ✗ const CollisionPair & cp = geom_model.collisionPairs[k];
505
506 Eigen::DenseIndex i, j;
507 ✗ if (upper)
508 {
509 ✗ j = (Eigen::DenseIndex)std::max(cp.first, cp.second);
510 ✗ i = (Eigen::DenseIndex)std::min(cp.first, cp.second);
511 }
512 else
513 {
514 ✗ i = (Eigen::DenseIndex)std::max(cp.first, cp.second);
515 ✗ j = (Eigen::DenseIndex)std::min(cp.first, cp.second);
516 }
517
518 ✗ collisionRequests[k].security_margin = security_margin_map(i, j);
519 ✗ if (sync_distance_upper_bound)
520 ✗ collisionRequests[k].distance_upper_bound = collisionRequests[k].security_margin;
521 }
522 }
523 #endif // ifdef PINOCCHIO_WITH_HPP_FCL
524
525 ✗ inline void GeometryData::deactivateCollisionPair(const PairIndex pair_id)
526 {
527 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(
528 pair_id < activeCollisionPairs.size(),
529 "The input argument pair_id is larger than the number of "
530 "collision pairs contained in activeCollisionPairs.");
531 ✗ activeCollisionPairs[pair_id] = false;
532 }
533
534 ✗ inline void GeometryData::deactivateAllCollisionPairs()
535 {
536 ✗ std::fill(activeCollisionPairs.begin(), activeCollisionPairs.end(), false);
537 }
538
539 } // namespace pinocchio
540
541 /// @endcond
542
543 #endif // ifndef __pinocchio_multibody_geometry_hxx__
544