GCC Code Coverage Report

Directory: ./
File: include/pinocchio/collision/broadphase-manager.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 37 0.0%
Branches: 0 42 0.0%
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 ✗ BroadPhaseManagerTpl(
43 const Model * model_ptr,
44 const GeometryModel * geometry_model_ptr,
45 GeometryData * geometry_data_ptr,
46 const GeometryObjectFilterBase & filter = GeometryObjectFilterNothing())
47 ✗ : Base(model_ptr, geometry_model_ptr, geometry_data_ptr)
48 {
49 ✗ const GeometryModel & geom_model = *geometry_model_ptr;
50 ✗ selected_geometry_objects = filter.apply(geom_model.geometryObjects);
51
52 ✗ geometry_to_collision_index.resize(
53 ✗ geometry_model_ptr->geometryObjects.size(), (std::numeric_limits<size_t>::max)());
54 ✗ collision_object_is_active.resize(selected_geometry_objects.size(), true);
55 ✗ for (size_t k = 0; k < selected_geometry_objects.size(); ++k)
56 {
57 ✗ const size_t geometry_id = selected_geometry_objects[k];
58 ✗ geometry_to_collision_index[geometry_id] = k;
59 ✗ collision_object_is_active[k] = !geom_model.geometryObjects[geometry_id].disableCollision;
60 }
61
62 ✗ selected_collision_pairs.reserve(geom_model.collisionPairs.size());
63 ✗ for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
64 {
65 ✗ const CollisionPair & pair = geom_model.collisionPairs[k];
66 ✗ if (
67 ✗ geometry_to_collision_index[pair.first] != (std::numeric_limits<size_t>::max)()
68 ✗ && geometry_to_collision_index[pair.second] != (std::numeric_limits<size_t>::max)())
69 {
70 ✗ selected_collision_pairs.push_back(k);
71 }
72
73 ✗ selected_collision_pairs.resize(selected_collision_pairs.size());
74 }
75
76 ✗ collision_object_inflation.resize(
77 ✗ static_cast<Eigen::DenseIndex>(selected_geometry_objects.size()));
78
79 ✗ init();
80 }
81
82 /// @brief Copy contructor.
83 ///
84 /// \param[in] other manager to copy.
85 ///
86 ✗ BroadPhaseManagerTpl(const BroadPhaseManagerTpl & other)
87 : Base(other)
88 ✗ , collision_object_inflation(other.collision_object_inflation.size())
89 ✗ , selected_geometry_objects(other.selected_geometry_objects)
90 ✗ , geometry_to_collision_index(other.geometry_to_collision_index)
91 ✗ , selected_collision_pairs(other.selected_collision_pairs)
92 ✗ , collision_object_is_active(other.collision_object_is_active)
93 {
94 ✗ init();
95 }
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 ✗ CollisionObjectVector & getCollisionObjects()
133 {
134 ✗ 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