GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/geometry.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 241 258 93.4%
Branches: 195 556 35.1%
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 7 inline CollisionPair::CollisionPair()
27 7 : Base((std::numeric_limits<GeomIndex>::max)(), (std::numeric_limits<GeomIndex>::max)())
28 {
29 7 }
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 6244 inline CollisionPair::CollisionPair(const GeomIndex co1, const GeomIndex co2)
39 6244 : Base(co1, co2)
40 {
41
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 6244 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 6244 PINOCCHIO_CHECK_INPUT_ARGUMENT(co1 != co2, "The index of collision objects must not be equal.");
42 6244 }
43
44 1156 inline bool CollisionPair::operator==(const CollisionPair & rhs) const
45 {
46
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1152 times.
 1153 return (first == rhs.first && second == rhs.second)
47
3/6
✓ Branch 0 taken 1153 times.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 2309 || (first == rhs.second && second == rhs.first);
48 }
49
50 4 inline bool CollisionPair::operator!=(const CollisionPair & other) const
51 {
52 4 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 1 inline GeometryModel GeometryModel::clone() const
67 {
68 1 GeometryModel res;
69 1 res.ngeoms = ngeoms;
70
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res.collisionPairs = collisionPairs;
71
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res.collisionPairMapping = collisionPairMapping;
72
73
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 res.geometryObjects.reserve(geometryObjects.size());
74
2/2
✓ Branch 4 taken 20 times.
✓ Branch 5 taken 1 times.
 21 for (const GeometryObject & geometry_object : geometryObjects)
75 {
76
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 res.geometryObjects.push_back(geometry_object.clone());
77 }
78
79 1 return res;
80 }
81
82 72 inline GeometryData::GeometryData(const GeometryModel & geom_model)
83
1/2
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
 72 : oMg(geom_model.ngeoms)
84
1/2
✓ Branch 3 taken 72 times.
✗ Branch 4 not taken.
 72 , activeCollisionPairs(geom_model.collisionPairs.size(), true)
85 #ifdef PINOCCHIO_WITH_HPP_FCL
86
2/4
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 72 times.
✗ Branch 7 not taken.
 72 , distanceRequests(geom_model.collisionPairs.size(), hpp::fcl::DistanceRequest(true))
87
1/2
✓ Branch 3 taken 72 times.
✗ Branch 4 not taken.
 72 , distanceResults(geom_model.collisionPairs.size())
88
1/2
✓ Branch 3 taken 72 times.
✗ Branch 4 not taken.
 144 , collisionRequests(
89
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 144 geom_model.collisionPairs.size(), hpp::fcl::CollisionRequest(::hpp::fcl::NO_REQUEST, 1))
90
1/2
✓ Branch 3 taken 72 times.
✗ Branch 4 not taken.
 72 , collisionResults(geom_model.collisionPairs.size())
91 72 , radius()
92 72 , collisionPairIndex(0)
93 #endif // PINOCCHIO_WITH_HPP_FCL
94 72 , innerObjects()
95
2/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
 144 , outerObjects()
96 {
97 #ifdef PINOCCHIO_WITH_HPP_FCL
98
18/30
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 72 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 72 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3491 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3491 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 3491 times.
✓ Branch 25 taken 3491 times.
✓ Branch 26 taken 3491 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 3491 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 3563 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 3563 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 3491 times.
✓ Branch 37 taken 72 times.
✓ Branch 38 taken 3491 times.
✓ Branch 39 taken 72 times.
 7054 BOOST_FOREACH (hpp::fcl::CollisionRequest & creq, collisionRequests)
99 {
100 3491 creq.enable_cached_gjk_guess = true;
101 }
102 #if HPP_FCL_VERSION_AT_LEAST(1, 4, 5)
103
18/30
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 72 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 72 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3491 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3491 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 3491 times.
✓ Branch 25 taken 3491 times.
✓ Branch 26 taken 3491 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 3491 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 3563 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 3563 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 3491 times.
✓ Branch 37 taken 72 times.
✓ Branch 38 taken 3491 times.
✓ Branch 39 taken 72 times.
 7054 BOOST_FOREACH (hpp::fcl::DistanceRequest & dreq, distanceRequests)
104 {
105 3491 dreq.enable_cached_gjk_guess = true;
106 }
107 #endif
108
1/2
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
 72 collision_functors.reserve(geom_model.collisionPairs.size());
109
1/2
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
 72 distance_functors.reserve(geom_model.collisionPairs.size());
110
111
2/2
✓ Branch 1 taken 3491 times.
✓ Branch 2 taken 72 times.
 3563 for (size_t cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index)
112 {
113 3491 const CollisionPair & cp = geom_model.collisionPairs[cp_index];
114 3491 const GeometryObject & obj_1 = geom_model.geometryObjects[cp.first];
115 3491 const GeometryObject & obj_2 = geom_model.geometryObjects[cp.second];
116
117
2/4
✓ Branch 1 taken 3491 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3491 times.
✗ Branch 5 not taken.
 3491 collision_functors.push_back(ComputeCollision(obj_1, obj_2));
118
2/4
✓ Branch 1 taken 3491 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3491 times.
✗ Branch 5 not taken.
 3491 distance_functors.push_back(ComputeDistance(obj_1, obj_2));
119 }
120 #endif
121
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 fillInnerOuterObjectMaps(geom_model);
122 72 }
123
124 40 inline GeometryData::GeometryData(const GeometryData & other)
125 40 : oMg(other.oMg)
126
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 , activeCollisionPairs(other.activeCollisionPairs)
127 #ifdef PINOCCHIO_WITH_HPP_FCL
128
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 , distanceRequests(other.distanceRequests)
129
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 , distanceResults(other.distanceResults)
130
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 , collisionRequests(other.collisionRequests)
131
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 , collisionResults(other.collisionResults)
132
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 , radius(other.radius)
133 40 , collisionPairIndex(other.collisionPairIndex)
134
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 , collision_functors(other.collision_functors)
135
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 , distance_functors(other.distance_functors)
136 #endif // PINOCCHIO_WITH_HPP_FCL
137
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 , innerObjects(other.innerObjects)
138
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 , outerObjects(other.outerObjects)
139 {
140 40 }
141
142 1 inline GeometryData & GeometryData::operator=(const GeometryData & other)
143 {
144
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (this != &other)
145 {
146 1 oMg = other.oMg;
147 1 activeCollisionPairs = other.activeCollisionPairs;
148 #ifdef PINOCCHIO_WITH_HPP_FCL
149 1 distanceRequests = other.distanceRequests;
150 1 distanceResults = other.distanceResults;
151 1 collisionRequests = other.collisionRequests;
152 1 collisionResults = other.collisionResults;
153 1 radius = other.radius;
154 1 collisionPairIndex = other.collisionPairIndex;
155 1 collision_functors = other.collision_functors;
156 1 distance_functors = other.distance_functors;
157 #endif // PINOCCHIO_WITH_HPP_FCL
158 1 innerObjects = other.innerObjects;
159 1 outerObjects = other.outerObjects;
160 }
161 1 return *this;
162 }
163
164 126 inline GeometryData::~GeometryData()
165 {
166 126 }
167
168 template<typename S2, int O2, template<typename, int> class JointCollectionTpl>
169 2 GeomIndex GeometryModel::addGeometryObject(
170 const GeometryObject & object, const ModelTpl<S2, O2, JointCollectionTpl> & model)
171 {
172
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 if (object.parentFrame < (FrameIndex)model.nframes)
173
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 2 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 2 Eigen::DenseIndex idx = (Eigen::DenseIndex)(ngeoms++);
178 2 geometryObjects.push_back(object);
179 2 geometryObjects.back().parentJoint = model.frames[object.parentFrame].parentJoint;
180
181 2 collisionPairMapping.conservativeResize(idx + 1, idx + 1);
182
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 collisionPairMapping.col(idx).fill(-1);
183
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 collisionPairMapping.row(idx).fill(-1);
184
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(collisionPairMapping.cols() == (Eigen::DenseIndex)ngeoms);
185
186 2 return (GeomIndex)idx;
187 }
188
189 2215 inline GeomIndex GeometryModel::addGeometryObject(const GeometryObject & object)
190 {
191 2215 Eigen::DenseIndex idx = (Eigen::DenseIndex)(ngeoms++);
192 2215 geometryObjects.push_back(object);
193
194 2215 collisionPairMapping.conservativeResize(idx + 1, idx + 1);
195
1/2
✓ Branch 2 taken 2215 times.
✗ Branch 3 not taken.
 2215 collisionPairMapping.col(idx).fill(-1);
196
1/2
✓ Branch 2 taken 2215 times.
✗ Branch 3 not taken.
 2215 collisionPairMapping.row(idx).fill(-1);
197
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2215 times.
 2215 assert(collisionPairMapping.cols() == (Eigen::DenseIndex)ngeoms);
198
199 2215 return (GeomIndex)idx;
200 }
201
202 1 inline void GeometryModel::removeGeometryObject(const std::string & name)
203 {
204 1 GeomIndex i = 0;
205 1 GeometryObjectVector::iterator it;
206
1/2
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 for (it = geometryObjects.begin(); it != geometryObjects.end(); ++it, ++i)
207 {
208
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 1 times.
 2 if (it->name == name)
209 {
210 1 break;
211 }
212 }
213
1/12
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
 1 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 1 for (CollisionPairVector::iterator itCol = collisionPairs.begin();
218
2/2
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 1 times.
 4 itCol != collisionPairs.end(); ++itCol)
219 {
220
6/6
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 2 times.
✓ Branch 7 taken 1 times.
 3 if ((itCol->first == i) || (itCol->second == i))
221 {
222
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 itCol = collisionPairs.erase(itCol);
223 2 itCol--;
224 }
225 else
226 {
227 // Indices of objects after the one that is removed should be decreased by one.
228
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (itCol->first > i)
229 ✗ itCol->first--;
230
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (itCol->second > i)
231 1 itCol->second--;
232 }
233 }
234
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 geometryObjects.erase(it);
235 1 ngeoms--;
236 1 }
237
238 2783 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 2783 times.
✗ Branch 4 not taken.
 2783 GeometryObjectVector::const_iterator it = std::find_if(
244 geometryObjects.begin(), geometryObjects.end(),
245
3/6
✓ Branch 1 taken 2783 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2783 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2783 times.
✗ Branch 8 not taken.
 5566 boost::bind(&GeometryObject::name, _1) == name);
246 2783 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 72 inline void GeometryData::fillInnerOuterObjectMaps(const GeometryModel & geomModel)
261 {
262 72 innerObjects.clear();
263 72 outerObjects.clear();
264
265
2/2
✓ Branch 1 taken 1025 times.
✓ Branch 2 taken 72 times.
 1097 for (GeomIndex gid = 0; gid < geomModel.geometryObjects.size(); gid++)
266
2/4
✓ Branch 2 taken 1025 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1025 times.
✗ Branch 6 not taken.
 1025 innerObjects[geomModel.geometryObjects[gid].parentJoint].push_back(gid);
267
268
18/30
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 72 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 72 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3491 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3491 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 3491 times.
✓ Branch 25 taken 3491 times.
✓ Branch 26 taken 3491 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 3491 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 3563 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 3563 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 3491 times.
✓ Branch 37 taken 72 times.
✓ Branch 38 taken 3491 times.
✓ Branch 39 taken 72 times.
 7054 BOOST_FOREACH (const CollisionPair & pair, geomModel.collisionPairs)
269 {
270
2/4
✓ Branch 2 taken 3491 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3491 times.
✗ Branch 6 not taken.
 3491 outerObjects[geomModel.geometryObjects[pair.first].parentJoint].push_back(pair.second);
271 }
272 72 }
273
274 1 inline std::ostream & operator<<(std::ostream & os, const GeometryModel & geomModel)
275 {
276 1 os << "Nb geometry objects = " << geomModel.ngeoms << std::endl;
277
278
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 4 for (GeomIndex i = 0; i < (GeomIndex)(geomModel.ngeoms); ++i)
279 {
280 3 os << geomModel.geometryObjects[i] << std::endl;
281 }
282
283 1 return os;
284 }
285
286 1 inline std::ostream & operator<<(std::ostream & os, const GeometryData & geomData)
287 {
288 #ifdef PINOCCHIO_WITH_HPP_FCL
289 1 os << "Number of collision pairs = " << geomData.activeCollisionPairs.size() << std::endl;
290
291
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 4 for (PairIndex i = 0; i < (PairIndex)(geomData.activeCollisionPairs.size()); ++i)
292 {
293
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 os << "Pairs " << i << (geomData.activeCollisionPairs[i] ? " active" : " inactive")
294 3 << 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 1 os << "Number of geometry objects = " << geomData.oMg.size() << std::endl;
302
303 1 return os;
304 }
305
306 5667 inline void GeometryModel::addCollisionPair(const CollisionPair & pair)
307 {
308
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 5667 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 5667 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 5667 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 5667 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 5667 times.
✗ Branch 2 not taken.
 5667 if (!existCollisionPair(pair))
315 {
316 5667 collisionPairs.push_back(pair);
317
318 11334 collisionPairMapping((Eigen::DenseIndex)pair.second, (Eigen::DenseIndex)pair.first) =
319 5667 (int)(collisionPairs.size() - 1);
320 5667 collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second) =
321 5667 collisionPairMapping(
322 5667 (Eigen::DenseIndex)pair.second,
323 5667 (Eigen::DenseIndex)pair.first); // make symmetric
324 }
325 5667 }
326
327 2 inline void GeometryModel::setCollisionPairs(const MatrixXb & map, const bool upper)
328 {
329
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(
330 map.rows(), (Eigen::DenseIndex)ngeoms, "Input map does not have the correct number of rows.");
331
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(
332 map.cols(), (Eigen::DenseIndex)ngeoms,
333 "Input map does not have the correct number of columns.");
334 2 removeAllCollisionPairs();
335
336
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 2 times.
 42 for (Eigen::DenseIndex i = 0; i < (Eigen::DenseIndex)ngeoms; ++i)
337 {
338
2/2
✓ Branch 0 taken 380 times.
✓ Branch 1 taken 40 times.
 420 for (Eigen::DenseIndex j = i + 1; j < (Eigen::DenseIndex)ngeoms; ++j)
339 {
340
2/2
✓ Branch 0 taken 190 times.
✓ Branch 1 taken 190 times.
 380 if (upper)
341 {
342
1/2
✓ Branch 1 taken 190 times.
✗ Branch 2 not taken.
 190 if (map(i, j))
343
1/2
✓ Branch 2 taken 190 times.
✗ Branch 3 not taken.
 190 addCollisionPair(CollisionPair((std::size_t)i, (std::size_t)j));
344 }
345 else
346 {
347
1/2
✓ Branch 1 taken 190 times.
✗ Branch 2 not taken.
 190 if (map(j, i))
348
1/2
✓ Branch 2 taken 190 times.
✗ Branch 3 not taken.
 190 addCollisionPair(CollisionPair((std::size_t)i, (std::size_t)j));
349 }
350 }
351 }
352 2 }
353
354 15 inline void GeometryModel::addAllCollisionPairs()
355 {
356 15 removeAllCollisionPairs();
357
2/2
✓ Branch 0 taken 308 times.
✓ Branch 1 taken 15 times.
 323 for (GeomIndex i = 0; i < ngeoms; ++i)
358 {
359 308 const JointIndex joint_i = geometryObjects[i].parentJoint;
360
2/2
✓ Branch 0 taken 3785 times.
✓ Branch 1 taken 308 times.
 4093 for (GeomIndex j = i + 1; j < ngeoms; ++j)
361 {
362 3785 const JointIndex joint_j = geometryObjects[j].parentJoint;
363
2/2
✓ Branch 0 taken 3689 times.
✓ Branch 1 taken 96 times.
 3785 if (joint_i != joint_j)
364
1/2
✓ Branch 2 taken 3689 times.
✗ Branch 3 not taken.
 3689 addCollisionPair(CollisionPair(i, j));
365 }
366 }
367 15 }
368
369 1188 inline void GeometryModel::removeCollisionPair(const CollisionPair & pair)
370 {
371
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1188 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 1188 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 1188 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 1188 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 1188 const long index = (long)findCollisionPair(pair);
379
380
1/2
✓ Branch 1 taken 1188 times.
✗ Branch 2 not taken.
 1188 if (index != (long)collisionPairs.size())
381 {
382 2376 collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second) =
383 1188 collisionPairMapping((Eigen::DenseIndex)pair.second, (Eigen::DenseIndex)pair.first) = -1;
384
1/2
✓ Branch 4 taken 1188 times.
✗ Branch 5 not taken.
 1188 collisionPairs.erase(collisionPairs.begin() + index);
385
386
2/2
✓ Branch 1 taken 34896 times.
✓ Branch 2 taken 1188 times.
 36084 for (Eigen::DenseIndex i = 0; i < collisionPairMapping.cols(); ++i)
387 {
388
2/2
✓ Branch 1 taken 621048 times.
✓ Branch 2 taken 34896 times.
 655944 for (Eigen::DenseIndex j = i + 1; j < collisionPairMapping.cols(); ++j)
389 {
390
2/2
✓ Branch 1 taken 237425 times.
✓ Branch 2 taken 383623 times.
 621048 if (collisionPairMapping(i, j) > index)
391 {
392 237425 collisionPairMapping(i, j)--;
393 237425 collisionPairMapping(j, i) = collisionPairMapping(i, j);
394 }
395 }
396 }
397 }
398 1188 }
399
400 17 inline void GeometryModel::removeAllCollisionPairs()
401 {
402 17 collisionPairs.clear();
403
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 collisionPairMapping.fill(-1);
404 17 }
405
406 6427 inline bool GeometryModel::existCollisionPair(const CollisionPair & pair) const
407 {
408 6427 return collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second)
409 6427 != -1;
410 }
411
412 591369 inline PairIndex GeometryModel::findCollisionPair(const CollisionPair & pair) const
413 {
414 591369 int res = collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second);
415
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 591369 times.
 591369 if (res == -1)
416 ✗ return collisionPairs.size();
417 else
418 591369 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 2 inline void GeometryData::activateAllCollisionPairs()
431 {
432
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 std::fill(activeCollisionPairs.begin(), activeCollisionPairs.end(), true);
433 2 }
434
435 2 inline void GeometryData::setActiveCollisionPairs(
436 const GeometryModel & geom_model, const MatrixXb & map, const bool upper)
437 {
438 2 const Eigen::DenseIndex ngeoms = (Eigen::DenseIndex)geom_model.ngeoms;
439
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(
440 map.rows(), ngeoms, "Input map does not have the correct number of rows.");
441
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(
442 map.cols(), ngeoms, "Input map does not have the correct number of columns.");
443
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 2 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
2/2
✓ Branch 1 taken 380 times.
✓ Branch 2 taken 2 times.
 382 for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
448 {
449 380 const CollisionPair & cp = geom_model.collisionPairs[k];
450
451 Eigen::DenseIndex i, j;
452
2/2
✓ Branch 0 taken 190 times.
✓ Branch 1 taken 190 times.
 380 if (upper)
453 {
454 190 j = (Eigen::DenseIndex)std::max(cp.first, cp.second);
455 190 i = (Eigen::DenseIndex)std::min(cp.first, cp.second);
456 }
457 else
458 {
459 190 i = (Eigen::DenseIndex)std::max(cp.first, cp.second);
460 190 j = (Eigen::DenseIndex)std::min(cp.first, cp.second);
461 }
462
463 380 activeCollisionPairs[k] = map(i, j);
464 }
465 2 }
466
467 2 inline void GeometryData::setGeometryCollisionStatus(
468 const GeometryModel & geom_model, const GeomIndex geom_id, const bool enable_collision)
469 {
470
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 2 PINOCCHIO_CHECK_INPUT_ARGUMENT(
471 geom_id < geom_model.ngeoms, "The index of the geometry is not valid");
472
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 2 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
2/2
✓ Branch 1 taken 380 times.
✓ Branch 2 taken 2 times.
 382 for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
477 {
478 380 const CollisionPair & cp = geom_model.collisionPairs[k];
479
3/4
✓ Branch 0 taken 342 times.
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 342 times.
 380 if (cp.first == geom_id || cp.second == geom_id)
480 {
481 38 activeCollisionPairs[k] = enable_collision;
482 }
483 }
484 2 }
485
486 #ifdef PINOCCHIO_WITH_HPP_FCL
487 2 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 2 const Eigen::DenseIndex ngeoms = (Eigen::DenseIndex)geom_model.ngeoms;
494
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(
495 security_margin_map.rows(), ngeoms, "Input map does not have the correct number of rows.");
496
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(
497 security_margin_map.cols(), ngeoms, "Input map does not have the correct number of columns.");
498
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 2 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
2/2
✓ Branch 1 taken 380 times.
✓ Branch 2 taken 2 times.
 382 for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
503 {
504 380 const CollisionPair & cp = geom_model.collisionPairs[k];
505
506 Eigen::DenseIndex i, j;
507
2/2
✓ Branch 0 taken 190 times.
✓ Branch 1 taken 190 times.
 380 if (upper)
508 {
509 190 j = (Eigen::DenseIndex)std::max(cp.first, cp.second);
510 190 i = (Eigen::DenseIndex)std::min(cp.first, cp.second);
511 }
512 else
513 {
514 190 i = (Eigen::DenseIndex)std::max(cp.first, cp.second);
515 190 j = (Eigen::DenseIndex)std::min(cp.first, cp.second);
516 }
517
518 380 collisionRequests[k].security_margin = security_margin_map(i, j);
519
2/2
✓ Branch 0 taken 190 times.
✓ Branch 1 taken 190 times.
 380 if (sync_distance_upper_bound)
520 190 collisionRequests[k].distance_upper_bound = collisionRequests[k].security_margin;
521 }
522 2 }
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 4 inline void GeometryData::deactivateAllCollisionPairs()
535 {
536
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 4 std::fill(activeCollisionPairs.begin(), activeCollisionPairs.end(), false);
537 4 }
538
539 } // namespace pinocchio
540
541 /// @endcond
542
543 #endif // ifndef __pinocchio_multibody_geometry_hxx__
544