Collision Checker
collision_solver_fcl.cc
Go to the documentation of this file.
1 #include "collision/solvers/collision_solver_fcl.h"
2 
3 #include "collision/collision_object_types.h"
4 #include "collision/solvers/collision_solvers.h"
5 #include "collision/solvers/fcl/fcl_collision_queries.h"
6 #include "collision/solvers/fcl/fcl_helpers.h"
7 
8 namespace collision {
9 
10 namespace solvers {
11 namespace solverFCL {
12 
13 std::size_t collide_obj_obj(const CollisionObject &obj1,
14  const CollisionObject &obj2, CollisionResult &res,
15  const CollisionRequest &req) {
16  const FCLCollisionObject *obj1_fcl = get_fcl_object_ptr(&obj1);
17  const FCLCollisionObject *obj2_fcl = get_fcl_object_ptr(&obj2);
18  if (!obj1_fcl || !obj2_fcl) return -1;
19 
20  res.set_result(fcl_primitive_queries::fcl_generic_collisionDetection(
21  *obj1_fcl, *obj2_fcl));
22  return 0;
23 }
24 std::size_t collide_shape_group_obj(const CollisionObject &obj1,
25  const CollisionObject &obj2,
26  CollisionResult &res,
27  const CollisionRequest &req) {
28  const FCLCollisionObjectGroup *obj1_fcl = get_fcl_object_group_ptr(&obj1);
29  const FCLCollisionObject *obj2_fcl = get_fcl_object_ptr(&obj2);
30  if (!obj1_fcl || !obj2_fcl) return -1;
31  res.set_result(fcl_primitive_queries::fcl_generic_collisionDetection(
32  *obj1_fcl, *obj2_fcl));
33  return 0;
34 }
35 std::size_t collide_obj_shape_group(const CollisionObject &obj1,
36  const CollisionObject &obj2,
37  CollisionResult &res,
38  const CollisionRequest &req) {
39  return collide_shape_group_obj(obj2, obj1, res, req);
40 }
41 std::size_t collide_shape_group_shape_group(const CollisionObject &obj1,
42  const CollisionObject &obj2,
43  CollisionResult &res,
44  const CollisionRequest &req) {
45  const FCLCollisionObjectGroup *obj1_fcl = get_fcl_object_group_ptr(&obj1);
46  const FCLCollisionObjectGroup *obj2_fcl = get_fcl_object_group_ptr(&obj2);
47 
48  if (!obj1_fcl || !obj2_fcl) return -1;
49  res.set_result(fcl_primitive_queries::fcl_generic_collisionDetection(
50  *obj1_fcl, *obj2_fcl));
51  return 0;
52 }
53 
54 std::size_t collide_tvobst_obj(const CollisionObject &obj1,
55  const CollisionObject &obj2,
56  CollisionResult &res,
57  const CollisionRequest &req) {
58  static solvers::FCLSolver solver;
59  static CollisionFunctionMatrix matr_fcl(&solver);
60  const TimeVariantCollisionObject &tvobst(
61  static_cast<const TimeVariantCollisionObject &>(obj1));
62  int time_idx_start = tvobst.time_start_idx();
63  int time_idx_end = tvobst.time_end_idx();
64  for (int cc1 = time_idx_start; cc1 <= time_idx_end; cc1++) {
65  CollisionObjectConstPtr cur_obj = tvobst.getObstacleAtTime(cc1);
66 
67  if (!cur_obj.get()) {
68  continue;
69  }
70 
71  if (cur_obj->getCollisionObjectClass() ==
72  CollisionObjectClass::OBJ_CLASS_TVOBSTACLE) {
73  return -1;
74  }
75 
76  collide_bool_func_t func = matr_fcl.getSolverBoolFunction(
77  cur_obj->getCollisionObjectType(), obj2.getCollisionObjectType());
78  if (!func) {
79  return -1;
80  }
81  func(*cur_obj, obj2, res, req);
82  if (res.collides()) return 0;
83  }
84  return 0;
85 }
86 
87 std::size_t collide_obj_tvobst(const CollisionObject &obj1,
88  const CollisionObject &obj2,
89  CollisionResult &res,
90  const CollisionRequest &req) {
91  return collide_tvobst_obj(obj2, obj1, res, req);
92 }
93 
94 std::size_t collide_tvobst_tvobst(const CollisionObject &obj1,
95  const CollisionObject &obj2,
96  CollisionResult &res,
97  const CollisionRequest &req) {
98  static solvers::FCLSolver solver;
99  static CollisionFunctionMatrix matr_fcl(&solver);
100  const TimeVariantCollisionObject &tvobst1(
101  static_cast<const TimeVariantCollisionObject &>(obj1));
102  const TimeVariantCollisionObject &tvobst2(
103  static_cast<const TimeVariantCollisionObject &>(obj2));
104  int time_idx_start =
105  std::min(tvobst1.time_start_idx(), tvobst2.time_start_idx());
106  int time_idx_end = std::max(tvobst1.time_end_idx(), tvobst2.time_end_idx());
107 
108  for (int cc1 = time_idx_start; cc1 <= time_idx_end; cc1++) {
109  CollisionObjectConstPtr obst_1 = tvobst1.getObstacleAtTime(cc1);
110  CollisionObjectConstPtr obst_2 = tvobst2.getObstacleAtTime(cc1);
111 
112  if (!obst_1.get() || !obst_2.get()) {
113  continue;
114  }
115  if (obst_1->getCollisionObjectClass() == OBJ_CLASS_TVOBSTACLE ||
116  obst_2->getCollisionObjectClass() == OBJ_CLASS_TVOBSTACLE) {
117  return -1;
118  }
119 
120  collide_bool_func_t func = matr_fcl.getSolverBoolFunction(
121  obst_1->getCollisionObjectType(), obst_2->getCollisionObjectType());
122  if (!func) {
123  return -1;
124  }
125  func(*obst_1, *obst_2, res, req);
126  if (res.collides()) return 0;
127  }
128  return 0;
129 }
130 
131 } // namespace solverFCL
132 
133 CollisionFunctionMatrix::CollisionFunctionMatrix(
134  const solvers::FCLSolver *solver) {
135  memset(m_collide_bool_function, 0,
136  COL_OBJECT_TYPES_COUNT * COL_OBJECT_TYPES_COUNT);
137  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_AABB_BOX] =
138  solvers::solverFCL::collide_obj_obj;
139  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_OBB_BOX] =
140  solvers::solverFCL::collide_obj_obj;
141  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_SPHERE] =
142  solvers::solverFCL::collide_obj_obj;
143  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_TRIANGLE] =
144  solvers::solverFCL::collide_obj_obj;
145  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_POINT] =
146  solvers::solverFCL::collide_obj_obj;
147  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_POLYGON] =
148  solvers::solverFCL::collide_obj_obj;
149  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_SHAPEGROUP] =
150  solvers::solverFCL::collide_obj_shape_group;
151  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_TVOBSTACLE] =
152  solvers::solverFCL::collide_obj_tvobst;
153 
154  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_AABB_BOX] =
155  solvers::solverFCL::collide_obj_obj;
156  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_OBB_BOX] =
157  solvers::solverFCL::collide_obj_obj;
158  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_SPHERE] =
159  solvers::solverFCL::collide_obj_obj;
160  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_TRIANGLE] =
161  solvers::solverFCL::collide_obj_obj;
162  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_POINT] =
163  solvers::solverFCL::collide_obj_obj;
164  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_POLYGON] =
165  solvers::solverFCL::collide_obj_obj;
166  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_SHAPEGROUP] =
167  solvers::solverFCL::collide_obj_shape_group;
168  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_TVOBSTACLE] =
169  solvers::solverFCL::collide_obj_tvobst;
170 
171  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_AABB_BOX] =
172  solvers::solverFCL::collide_obj_obj;
173  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_OBB_BOX] =
174  solvers::solverFCL::collide_obj_obj;
175  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_SPHERE] =
176  solvers::solverFCL::collide_obj_obj;
177  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_TRIANGLE] =
178  solvers::solverFCL::collide_obj_obj;
179  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_POINT] =
180  solvers::solverFCL::collide_obj_obj;
181  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_POLYGON] =
182  solvers::solverFCL::collide_obj_obj;
183  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_SHAPEGROUP] =
184  solvers::solverFCL::collide_obj_shape_group;
185  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_TVOBSTACLE] =
186  solvers::solverFCL::collide_obj_tvobst;
187 
188  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_AABB_BOX] =
189  solvers::solverFCL::collide_obj_obj;
190  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_OBB_BOX] =
191  solvers::solverFCL::collide_obj_obj;
192  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_SPHERE] =
193  solvers::solverFCL::collide_obj_obj;
194  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_TRIANGLE] =
195  solvers::solverFCL::collide_obj_obj;
196  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_POINT] =
197  solvers::solverFCL::collide_obj_obj;
198  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_POLYGON] =
199  solvers::solverFCL::collide_obj_obj;
200  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_SHAPEGROUP] =
201  solvers::solverFCL::collide_obj_shape_group;
202  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_TVOBSTACLE] =
203  solvers::solverFCL::collide_obj_tvobst;
204 
205  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_AABB_BOX] =
206  solvers::solverFCL::collide_obj_obj;
207  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_OBB_BOX] =
208  solvers::solverFCL::collide_obj_obj;
209  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_SPHERE] =
210  solvers::solverFCL::collide_obj_obj;
211  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_TRIANGLE] =
212  solvers::solverFCL::collide_obj_obj;
213  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_POINT] =
214  solvers::solverFCL::collide_obj_obj;
215  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_POLYGON] =
216  solvers::solverFCL::collide_obj_obj;
217  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_SHAPEGROUP] =
218  solvers::solverFCL::collide_obj_shape_group;
219  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_TVOBSTACLE] =
220  solvers::solverFCL::collide_obj_tvobst;
221 
222  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_AABB_BOX] =
223  solvers::solverFCL::collide_obj_obj;
224  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_OBB_BOX] =
225  solvers::solverFCL::collide_obj_obj;
226  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_SPHERE] =
227  solvers::solverFCL::collide_obj_obj;
228  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_TRIANGLE] =
229  solvers::solverFCL::collide_obj_obj;
230  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_POINT] =
231  solvers::solverFCL::collide_obj_obj;
232  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_POLYGON] =
233  solvers::solverFCL::collide_obj_obj;
234  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_SHAPEGROUP] =
235  solvers::solverFCL::collide_obj_shape_group;
236  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_TVOBSTACLE] =
237  solvers::solverFCL::collide_obj_tvobst;
238 
239  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_AABB_BOX] =
240  solvers::solverFCL::collide_shape_group_obj;
241  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_OBB_BOX] =
242  solvers::solverFCL::collide_shape_group_obj;
243  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_SPHERE] =
244  solvers::solverFCL::collide_shape_group_obj;
245  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_TRIANGLE] =
246  solvers::solverFCL::collide_shape_group_obj;
247  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_POINT] =
248  solvers::solverFCL::collide_shape_group_obj;
249  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_POLYGON] =
250  solvers::solverFCL::collide_shape_group_obj;
251  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_SHAPEGROUP] =
252  solvers::solverFCL::collide_shape_group_shape_group;
253  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_TVOBSTACLE] =
254  solvers::solverFCL::collide_obj_tvobst;
255 
256  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_AABB_BOX] =
257  solvers::solverFCL::collide_tvobst_obj;
258  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_OBB_BOX] =
259  solvers::solverFCL::collide_tvobst_obj;
260  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_SPHERE] =
261  solvers::solverFCL::collide_tvobst_obj;
262  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_TRIANGLE] =
263  solvers::solverFCL::collide_tvobst_obj;
264  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_POINT] =
265  solvers::solverFCL::collide_tvobst_obj;
266  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_POLYGON] =
267  solvers::solverFCL::collide_tvobst_obj;
268  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_SHAPEGROUP] =
269  solvers::solverFCL::collide_tvobst_obj;
270  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_TVOBSTACLE] =
271  solvers::solverFCL::collide_tvobst_tvobst;
272 }
273 
274 } // namespace solvers
275 
276 } // namespace collision
fcl_collision_queries.h
collision::solvers::CollisionFunctionMatrix::getSolverBoolFunction
collide_bool_func_t getSolverBoolFunction(CollisionObjectType obj1_type, CollisionObjectType obj2_type) const
Definition: collision_solvers.h:23
collision::solvers::solverFCL::collide_tvobst_tvobst
std::size_t collide_tvobst_tvobst(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_fcl.cc:94
collision::TimeVariantCollisionObject
TimeVariantCollisionObject can contain a different CollisionObject or ShapeGroup at each time step...
Definition: time_variant_collision_object.h:30
collision_solvers.h
collision_object_types.h
collision::solvers::solverFCL::collide_shape_group_shape_group
std::size_t collide_shape_group_shape_group(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_fcl.cc:41
collision::solvers::collide_bool_func_t
std::size_t(* collide_bool_func_t)(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solvers.h:13
collision::CollisionObjectConstPtr
std::shared_ptr< const CollisionObject > CollisionObjectConstPtr
Definition: collision_object.h:41
collision::solvers::solverFCL::get_fcl_object_ptr
const FCLCollisionObject * get_fcl_object_ptr(const CollisionObject *obj)
Definition: fcl_helpers.h:40
collision::CollisionRequest
Universal structure specifying collision request properties.
Definition: collision_requests.h:24
collision::solvers::solverFCL::collide_obj_obj
std::size_t collide_obj_obj(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_fcl.cc:13
collision::solvers::solverFCL::collide_obj_shape_group
std::size_t collide_obj_shape_group(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_fcl.cc:35
collision::solvers::solverFCL::fcl_primitive_queries::fcl_generic_collisionDetection
bool fcl_generic_collisionDetection(const FCLCollisionObject &obj_first, const FCLCollisionObject &obj_second)
Definition: fcl_collision_queries.h:81
collision::CollisionObject::getCollisionObjectType
virtual CollisionObjectType getCollisionObjectType() const
Definition: collision_object.h:60
collision::solvers::solverFCL::collide_shape_group_obj
std::size_t collide_shape_group_obj(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_fcl.cc:24
collision::solvers::solverFCL::collide_obj_tvobst
std::size_t collide_obj_tvobst(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_fcl.cc:87
collision_solver_fcl.h
collision::solvers::CollisionFunctionMatrix::CollisionFunctionMatrix
CollisionFunctionMatrix(const solvers::FCLSolver *solver)
Definition: collision_solver_fcl.cc:133
collision::TimeVariantCollisionObject::getObstacleAtTime
CollisionObjectConstPtr getObstacleAtTime(int time_idx) const
Definition: time_variant_collision_object.cc:36
fcl_helpers.h
collision::CollisionObject
Base class for CollisionObjects and some of their groups.
Definition: collision_object.h:47
collision::solvers::solverFCL::collide_tvobst_obj
std::size_t collide_tvobst_obj(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_fcl.cc:54
collision::CollisionResult
Structure holding result for a collision request.
Definition: collision_requests.h:10
collision::solvers::solverFCL::get_fcl_object_group_ptr
const FCLCollisionObjectGroup * get_fcl_object_group_ptr(const CollisionObject *obj)
Definition: fcl_helpers.h:27
collision::COL_OBJECT_TYPES_COUNT
constexpr int COL_OBJECT_TYPES_COUNT
Definition: collision_object_types.h:13