GCC Code Coverage Report

Directory: ./
File: include/pinocchio/collision/broadphase-manager.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 103 128 80.5%
Branches: 69 134 51.5%
Line Branch Exec Source
1 //
2 // Copyright (c) 2022 INRIA
3 //
4
5 #ifndef __pinocchio_collision_broadphase_manager_hxx__
6 #define __pinocchio_collision_broadphase_manager_hxx__
7
8 namespace pinocchio
9 {
10
11 template<typename Manager>
12 34069 void BroadPhaseManagerTpl<Manager>::update(bool compute_local_aabb)
13 {
14 34069 const GeometryModel & geom_model = getGeometryModel();
15
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 34069 times.
 34069 assert(selected_geometry_objects.size() == size_t(collision_object_inflation.size()));
16
17 34069 GeometryData & geom_data = getGeometryData();
18
19 // Pass 1: check the new active geometries and list the new deactive geometries
20 34069 std::vector<size_t> new_active, new_inactive;
21
2/2
✓ Branch 1 taken 40082 times.
✓ Branch 2 taken 34069 times.
 74151 for (size_t k = 0; k < selected_geometry_objects.size(); ++k)
22 {
23 40082 const size_t geometry_object_id = selected_geometry_objects[k];
24 40082 const GeometryObject & geom_object = geom_model.geometryObjects[geometry_object_id];
25
26
2/4
✓ Branch 2 taken 40082 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 40082 times.
 40082 if (geom_object.geometry->aabb_local.volume() <= 0.) // degenerated geometry, we should not
27 // consider it as an active object.
28 {
29 ✗ if (collision_object_is_active[k])
30 {
31 ✗ collision_object_is_active[k] = false;
32 ✗ new_inactive.push_back(k);
33 }
34
35 ✗ continue; // don't go further and checks the next objects
36 }
37
38
2/4
✓ Branch 1 taken 40082 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 40082 times.
 40082 if (collision_object_is_active[k] != !geom_object.disableCollision) // change state
39 {
40 ✗ collision_object_is_active[k] = !geom_object.disableCollision;
41 ✗ if (geom_object.disableCollision)
42 ✗ new_inactive.push_back(k);
43 else
44 ✗ new_active.push_back(k);
45 }
46 }
47
48 // The pass should be done over all the geometry objects composing the collision tree.
49
1/2
✓ Branch 1 taken 34069 times.
✗ Branch 2 not taken.
 34069 collision_object_inflation.setZero();
50
2/2
✓ Branch 1 taken 2384760 times.
✓ Branch 2 taken 34069 times.
 2418829 for (size_t pair_id = 0; pair_id < geom_model.collisionPairs.size(); ++pair_id)
51 {
52 // TODO(jcarpent): enhance the performances by collecting only the collision pairs related to
53 // the selected_geometry_objects
54 2384760 const CollisionPair & pair = geom_model.collisionPairs[pair_id];
55 2384760 const GeomIndex geom1_id = pair.first;
56 2384760 const GeomIndex geom2_id = pair.second;
57
58 2384760 const bool geom1_is_selected =
59 2384760 geometry_to_collision_index[geom1_id] != (std::numeric_limits<size_t>::max)();
60 2384760 const bool geom2_is_selected =
61 2384760 geometry_to_collision_index[geom2_id] != (std::numeric_limits<size_t>::max)();
62
4/4
✓ Branch 0 taken 2244480 times.
✓ Branch 1 taken 140280 times.
✓ Branch 2 taken 2174340 times.
✓ Branch 3 taken 70140 times.
 2384760 if (!(geom1_is_selected || geom2_is_selected))
63 2174340 continue;
64
65 210420 const bool check_collision = geom_data.activeCollisionPairs[pair_id]
66
2/4
✓ Branch 1 taken 210420 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 210420 times.
✗ Branch 5 not taken.
 420840 && !(
67
1/2
✓ Branch 1 taken 210420 times.
✗ Branch 2 not taken.
 210420 geom_model.geometryObjects[geom1_id].disableCollision
68
1/2
✓ Branch 1 taken 210420 times.
✗ Branch 2 not taken.
 210420 || geom_model.geometryObjects[geom2_id].disableCollision);
69
70
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 210420 times.
 210420 if (!check_collision)
71 ✗ continue;
72
73 210420 const ::hpp::fcl::CollisionRequest & cr = geom_data.collisionRequests[pair_id];
74 210420 const double inflation = (cr.break_distance + cr.security_margin) * 0.5;
75
76
2/2
✓ Branch 0 taken 140280 times.
✓ Branch 1 taken 70140 times.
 210420 if (geom1_is_selected)
77 {
78 140280 const Eigen::DenseIndex geom1_id_local =
79 140280 static_cast<Eigen::DenseIndex>(geometry_to_collision_index[geom1_id]);
80
1/2
✓ Branch 1 taken 140280 times.
✗ Branch 2 not taken.
 140280 collision_object_inflation[geom1_id_local] =
81
1/2
✓ Branch 1 taken 140280 times.
✗ Branch 2 not taken.
 140280 (std::max)(inflation, collision_object_inflation[geom1_id_local]);
82 }
83
84
2/2
✓ Branch 0 taken 140280 times.
✓ Branch 1 taken 70140 times.
 210420 if (geom2_is_selected)
85 {
86 140280 const Eigen::DenseIndex geom2_id_local =
87 140280 static_cast<Eigen::DenseIndex>(geometry_to_collision_index[geom2_id]);
88
1/2
✓ Branch 1 taken 140280 times.
✗ Branch 2 not taken.
 140280 collision_object_inflation[geom2_id_local] =
89
1/2
✓ Branch 1 taken 140280 times.
✗ Branch 2 not taken.
 140280 (std::max)(inflation, collision_object_inflation[geom2_id_local]);
90 }
91 }
92
93
2/2
✓ Branch 3 taken 40082 times.
✓ Branch 4 taken 34069 times.
 114233 for (size_t k = 0; k < selected_geometry_objects.size(); ++k)
94 {
95
2/4
✓ Branch 1 taken 40082 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 40082 times.
 40082 if (!collision_object_is_active[k])
96 ✗ continue;
97
98 40082 const size_t geometry_object_id = selected_geometry_objects[k];
99
100 40082 const GeometryObject & geom_obj = geom_model.geometryObjects[geometry_object_id];
101 40082 hpp::fcl::CollisionGeometryPtr_t new_geometry = geom_obj.geometry;
102
103 40082 CollisionObject & collision_obj = collision_objects[k];
104 40082 hpp::fcl::CollisionGeometryPtr_t geometry = collision_obj.collisionGeometry();
105
106
2/4
✓ Branch 2 taken 40082 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 40082 times.
✗ Branch 6 not taken.
 40082 collision_obj.setTransform(toFclTransform3f(geom_data.oMg[geometry_object_id]));
107
108
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 40081 times.
 40082 if (new_geometry.get() != geometry.get())
109
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 collision_obj.setCollisionGeometry(new_geometry, compute_local_aabb);
110
111
1/2
✓ Branch 1 taken 40082 times.
✗ Branch 2 not taken.
 40082 collision_obj.computeAABB();
112
2/4
✓ Branch 2 taken 40082 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 40082 times.
✗ Branch 6 not taken.
 40082 collision_obj.getAABB().expand(collision_object_inflation[static_cast<Eigen::DenseIndex>(k)]);
113 }
114
115 // Remove deactivated collision objects
116
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 34069 times.
 34069 for (size_t k : new_inactive)
117 ✗ manager.unregisterObject(&collision_objects[k]);
118
119 // Add deactivated collision objects
120
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 34069 times.
 34069 for (size_t k : new_active)
121 ✗ manager.registerObject(&collision_objects[k]);
122
123
1/2
✓ Branch 1 taken 34069 times.
✗ Branch 2 not taken.
 34069 manager.update(); // because the position has changed.
124
2/4
✓ Branch 1 taken 34069 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 34069 times.
✗ Branch 4 not taken.
 34069 assert(check() && "The status of the BroadPhaseManager is not valid");
125 34069 }
126
127 template<typename Manager>
128 ✗ void BroadPhaseManagerTpl<Manager>::update(GeometryData * geom_data_ptr_new)
129 {
130 ✗ Base::geometry_data_ptr = geom_data_ptr_new;
131 ✗ update(false);
132 }
133
134 template<typename Manager>
135 104 BroadPhaseManagerTpl<Manager>::~BroadPhaseManagerTpl()
136 {
137 104 }
138
139 template<typename Manager>
140 34107 bool BroadPhaseManagerTpl<Manager>::check() const
141 {
142
1/2
✓ Branch 1 taken 34107 times.
✗ Branch 2 not taken.
 34107 std::vector<hpp::fcl::CollisionObject *> collision_objects_ptr = manager.getObjects();
143
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 34107 times.
 34107 if (collision_objects_ptr.size() > collision_objects.size())
144 ✗ return false;
145
146 34107 size_t count_active_objects = 0;
147
2/2
✓ Branch 4 taken 40108 times.
✓ Branch 5 taken 34107 times.
 74215 for (auto active : collision_object_is_active)
148 40108 count_active_objects += active;
149
150
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 34107 times.
 34107 if (count_active_objects != collision_objects_ptr.size())
151 ✗ return false;
152
153 34107 const GeometryModel & geom_model = getGeometryModel();
154
4/4
✓ Branch 1 taken 40106 times.
✓ Branch 2 taken 1 times.
✓ Branch 4 taken 40107 times.
✓ Branch 5 taken 34106 times.
 114320 for (size_t k = 0; k < selected_geometry_objects.size(); ++k)
155 {
156 40107 const size_t geometry_id = selected_geometry_objects[k];
157
158 40107 const hpp::fcl::CollisionObject & collision_obj = collision_objects[k];
159 40107 hpp::fcl::CollisionGeometryConstPtr_t geometry = collision_obj.collisionGeometry();
160
161
2/4
✓ Branch 1 taken 40107 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 40107 times.
✗ Branch 4 not taken.
 40107 if (collision_object_is_active[k]) // The collision object is active
162 {
163 40107 if (
164
1/2
✓ Branch 3 taken 40107 times.
✗ Branch 4 not taken.
 40107 std::find(collision_objects_ptr.begin(), collision_objects_ptr.end(), &collision_obj)
165
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 40107 times.
 80214 == collision_objects_ptr.end())
166 1 return false;
167
168 40107 if (
169
1/2
✓ Branch 2 taken 40107 times.
✗ Branch 3 not taken.
 40107 geometry.get()->aabb_local.volume()
170
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 40107 times.
 40107 == -(std::numeric_limits<hpp::fcl::FCL_REAL>::infinity)())
171 ✗ return false;
172
173 40107 const GeometryObject & geom_obj = geom_model.geometryObjects[geometry_id];
174 40107 hpp::fcl::CollisionGeometryConstPtr_t geometry_of_geom_obj = geom_obj.geometry;
175
176
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 40106 times.
 40107 if (geometry.get() != geometry_of_geom_obj.get())
177 1 return false;
178
2/2
✓ Branch 1 taken 40106 times.
✓ Branch 2 taken 1 times.
 40107 }
179 else
180 {
181 ✗ if (
182 ✗ std::find(collision_objects_ptr.begin(), collision_objects_ptr.end(), &collision_obj)
183 ✗ != collision_objects_ptr.end())
184 ✗ return false;
185 }
186 }
187
188 34106 return true;
189 34107 }
190
191 template<typename Manager>
192 34136 bool BroadPhaseManagerTpl<Manager>::check(CollisionCallBackBase * callback) const
193 {
194 34136 return &callback->getGeometryModel() == &getGeometryModel()
195
2/4
✓ Branch 0 taken 34136 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 34136 times.
✗ Branch 5 not taken.
 34136 && &callback->getGeometryData() == &getGeometryData();
196 }
197
198 template<typename Manager>
199 104 void BroadPhaseManagerTpl<Manager>::init()
200 {
201 104 const GeometryModel & geom_model = getGeometryModel();
202 104 collision_objects.reserve(selected_geometry_objects.size());
203
2/2
✓ Branch 1 taken 82 times.
✓ Branch 2 taken 104 times.
 186 for (size_t k = 0; k < selected_geometry_objects.size(); ++k)
204 {
205 82 const size_t geometry_id = selected_geometry_objects[k];
206 GeometryObject & geom_obj =
207 82 const_cast<GeometryObject &>(geom_model.geometryObjects[geometry_id]);
208
1/2
✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
 82 collision_objects.push_back(CollisionObject(geom_obj.geometry, geometry_id));
209
210 // Feed the base broadphase manager
211
1/2
✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
 82 if (collision_object_is_active[k])
212 82 manager.registerObject(&collision_objects.back());
213 }
214 104 }
215
216 template<typename Manager>
217 ✗ bool BroadPhaseManagerTpl<Manager>::collide(
218 CollisionObject & obj, CollisionCallBackBase * callback) const
219 {
220 ✗ manager.collide(&obj, callback);
221 ✗ return callback->collision;
222 }
223
224 template<typename Manager>
225 1004 bool BroadPhaseManagerTpl<Manager>::collide(CollisionCallBackBase * callback) const
226 {
227 1004 manager.collide(callback);
228 1004 return callback->collision;
229 }
230
231 template<typename Manager>
232 530112 bool BroadPhaseManagerTpl<Manager>::collide(
233 BroadPhaseManagerTpl & other_manager, CollisionCallBackBase * callback) const
234 {
235 530112 manager.collide(&other_manager.manager, callback);
236 530112 return callback->collision;
237 }
238
239 } // namespace pinocchio
240
241 #endif // ifndef __pinocchio_collision_broadphase_manager_hxx__
242