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 |
|
|
|