GCC Code Coverage Report

Directory: ./
File: include/pinocchio/collision/broadphase-manager.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 33 39 84.6%
Branches: 26 42 61.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2022 INRIA
3 //
4
5 #ifndef __pinocchio_collision_broadphase_manager_hpp__
6 #define __pinocchio_collision_broadphase_manager_hpp__
7
8 #include <hpp/fcl/broadphase/broadphase_collision_manager.h>
9
10 #include "pinocchio/collision/broadphase-manager-base.hpp"
11 #include "pinocchio/multibody/geometry-object-filter.hpp"
12
13 namespace pinocchio
14 {
15
16 template<typename _Manager>
17 struct BroadPhaseManagerTpl : public BroadPhaseManagerBase<BroadPhaseManagerTpl<_Manager>>
18 {
19 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
20
21 typedef BroadPhaseManagerBase<BroadPhaseManagerTpl<_Manager>> Base;
22 typedef std::vector<CollisionObject> CollisionObjectVector;
23 typedef Eigen::VectorXd VectorXs;
24 typedef _Manager Manager;
25
26 typedef ::pinocchio::Model Model;
27 typedef ::pinocchio::GeometryModel GeometryModel;
28 typedef ::pinocchio::GeometryData GeometryData;
29
30 /// @brief Default constructor.
31 BroadPhaseManagerTpl() // for std::vector
32 : Base()
33 {
34 }
35
36 /// @brief Constructor from a given geometry model and data.
37 ///
38 /// \param[in] model_ptr pointer to the model.
39 /// \param[in] geometry_model_ptr pointer to the geometry model.
40 /// \param[in] geometry_data_ptr pointer to the geometry data.
41 ///
42 70 BroadPhaseManagerTpl(
43 const Model * model_ptr,
44 const GeometryModel * geometry_model_ptr,
45 GeometryData * geometry_data_ptr,
46 const GeometryObjectFilterBase & filter = GeometryObjectFilterNothing())
47
1/2
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 : Base(model_ptr, geometry_model_ptr, geometry_data_ptr)
48 {
49 70 const GeometryModel & geom_model = *geometry_model_ptr;
50
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 selected_geometry_objects = filter.apply(geom_model.geometryObjects);
51
52
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 geometry_to_collision_index.resize(
53 70 geometry_model_ptr->geometryObjects.size(), (std::numeric_limits<size_t>::max)());
54
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 collision_object_is_active.resize(selected_geometry_objects.size(), true);
55
2/2
✓ Branch 1 taken 62 times.
✓ Branch 2 taken 70 times.
 132 for (size_t k = 0; k < selected_geometry_objects.size(); ++k)
56 {
57 62 const size_t geometry_id = selected_geometry_objects[k];
58 62 geometry_to_collision_index[geometry_id] = k;
59
1/2
✓ Branch 2 taken 62 times.
✗ Branch 3 not taken.
 62 collision_object_is_active[k] = !geom_model.geometryObjects[geometry_id].disableCollision;
60 }
61
62
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 selected_collision_pairs.reserve(geom_model.collisionPairs.size());
63
2/2
✓ Branch 1 taken 4690 times.
✓ Branch 2 taken 70 times.
 4760 for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
64 {
65 4690 const CollisionPair & pair = geom_model.collisionPairs[k];
66 4690 if (
67 4690 geometry_to_collision_index[pair.first] != (std::numeric_limits<size_t>::max)()
68
6/6
✓ Branch 0 taken 210 times.
✓ Branch 1 taken 4480 times.
✓ Branch 4 taken 70 times.
✓ Branch 5 taken 140 times.
✓ Branch 6 taken 70 times.
✓ Branch 7 taken 4620 times.
 4690 && geometry_to_collision_index[pair.second] != (std::numeric_limits<size_t>::max)())
69 {
70
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 selected_collision_pairs.push_back(k);
71 }
72
73
1/2
✓ Branch 2 taken 4690 times.
✗ Branch 3 not taken.
 4690 selected_collision_pairs.resize(selected_collision_pairs.size());
74 }
75
76
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 collision_object_inflation.resize(
77 70 static_cast<Eigen::DenseIndex>(selected_geometry_objects.size()));
78
79
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 init();
80 70 }
81
82 /// @brief Copy contructor.
83 ///
84 /// \param[in] other manager to copy.
85 ///
86 34 BroadPhaseManagerTpl(const BroadPhaseManagerTpl & other)
87 : Base(other)
88
1/2
✓ Branch 2 taken 34 times.
✗ Branch 3 not taken.
 34 , collision_object_inflation(other.collision_object_inflation.size())
89
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 , selected_geometry_objects(other.selected_geometry_objects)
90
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 , geometry_to_collision_index(other.geometry_to_collision_index)
91
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 , selected_collision_pairs(other.selected_collision_pairs)
92
1/2
✓ Branch 4 taken 34 times.
✗ Branch 5 not taken.
 68 , collision_object_is_active(other.collision_object_is_active)
93 {
94
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 init();
95 34 }
96
97 using Base::getGeometryData;
98 using Base::getGeometryModel;
99 using Base::getModel;
100
101 ///
102 /// @brief Update the manager from the current geometry positions and update the underlying FCL
103 /// broad phase manager.
104 ///
105 /// @param[in] compute_local_aabb whether to recompute the local AABB of the collision
106 /// geometries which have changed.
107 ///
108 void update(bool compute_local_aabb = false);
109
110 ///
111 /// @brief Update the manager with a new geometry data.
112 ///
113 /// \param[in] geom_data_ptr_new pointer to the new geometry data.
114 ///
115 void update(GeometryData * geom_data_ptr_new);
116
117 ~BroadPhaseManagerTpl();
118
119 /// @brief Check whether the base broad phase manager is aligned with the current
120 /// collision_objects.
121 bool check() const;
122
123 /// @brief Check whether the callback is inline with *this
124 bool check(CollisionCallBackBase * callback) const;
125
126 /// @brief Returns the vector of collision objects associated to the manager.
127 const CollisionObjectVector & getCollisionObjects() const
128 {
129 return collision_objects;
130 }
131 /// @brief Returns the vector of collision objects associated to the manager.
132 5 CollisionObjectVector & getCollisionObjects()
133 {
134 5 return collision_objects;
135 }
136
137 /// @brief Returns the inflation value related to each collision object.
138 ✗ const VectorXs & getCollisionObjectInflation()
139 {
140 ✗ return collision_object_inflation;
141 }
142
143 /// @brief Performs collision test between one object and all the objects belonging to the
144 /// manager.
145 bool collide(CollisionObject & obj, CollisionCallBackBase * callback) const;
146
147 /// @brief Performs collision test for the objects belonging to the manager.
148 bool collide(CollisionCallBackBase * callback) const;
149
150 /// @brief Performs collision test with objects belonging to another manager.
151 bool collide(BroadPhaseManagerTpl & other_manager, CollisionCallBackBase * callback) const;
152
153 // /// @brief Performs distance computation between one object and all the objects belonging to
154 // the manager void distance(CollisionObject* obj, DistanceCallBackBase * callback) const;
155
156 // /// @brief Performs distance test for the objects belonging to the manager (i.e., N^2 self
157 // distance) void distance(DistanceCallBackBase * callback) const;
158
159 // /// @brief Performs distance test with objects belonging to another manager
160 // void distance(BroadPhaseCollisionManager* other_manager, DistanceCallBackBase * callback)
161 // const;
162
163 /// @brief Returns internal manager.
164 ✗ Manager & getManager()
165 {
166 ✗ return manager;
167 }
168
169 /// @brief Returns internal manager.
170 const Manager & getManager() const
171 {
172 return manager;
173 }
174
175 /// @brief Returns the status of the collision object.
176 ✗ const std::vector<bool> & getCollisionObjectStatus() const
177 {
178 ✗ return collision_object_is_active;
179 }
180
181 protected:
182 /// @brief internal manager
183 Manager manager;
184
185 /// @brief the vector of collision objects.
186 CollisionObjectVector collision_objects;
187
188 /// @brief the inflation value related to each collision object.
189 VectorXs collision_object_inflation;
190
191 /// @brief Selected geometry objects in the original geometry_model.
192 std::vector<size_t> selected_geometry_objects;
193
194 /// @brief Mapping between a given geometry index and a collision index.
195 std::vector<size_t> geometry_to_collision_index;
196
197 /// @brief Selected collision pairs in the original geometry_model.
198 std::vector<size_t> selected_collision_pairs;
199
200 /// @brief Disable status related to each collision objects.
201 std::vector<bool> collision_object_is_active;
202
203 /// @brief Initialialisation of BroadPhaseManagerTpl
204 void init();
205
206 }; // struct BroadPhaseManagerTpl<BroadPhaseManagerDerived>
207
208 } // namespace pinocchio
209
210 /* --- Details -------------------------------------------------------------------- */
211 #include "pinocchio/collision/broadphase-manager.hxx"
212
213 #endif // ifndef __pinocchio_collision_broadphase_manager_hpp__
214