Collision Checker
collision_solver_default.cc
Go to the documentation of this file.
1 #include "collision/solvers/collision_solver_fcl.h"
2 #include "collision/solvers/collision_solver_primitive.h"
3 #include "collision/solvers/collision_solvers.h"
4 #include "collision/time_variant_collision_object.h"
5 
6 namespace collision {
7 namespace solvers {
8 namespace solverDefault {
9 
10 std::size_t collide_tvobst_obj(const CollisionObject &obj1,
11  const CollisionObject &obj2,
12  CollisionResult &res,
13  const CollisionRequest &req) {
14  static solvers::DefaultSolver solver;
15  static CollisionFunctionMatrix matr_default(&solver);
16  const TimeVariantCollisionObject &tvobst(
17  static_cast<const TimeVariantCollisionObject &>(obj1));
18  int time_idx_start = tvobst.time_start_idx();
19  int time_idx_end = tvobst.time_end_idx();
20  for (int cc1 = time_idx_start; cc1 <= time_idx_end; cc1++) {
21  CollisionObjectConstPtr cur_obj = tvobst.getObstacleAtTime(cc1);
22 
23  if (!cur_obj.get()) {
24  continue;
25  }
26 
27  if (cur_obj->getCollisionObjectClass() ==
28  CollisionObjectClass::OBJ_CLASS_TVOBSTACLE) {
29  return -1;
30  }
31 
32  collide_bool_func_t func = matr_default.getSolverBoolFunction(
33  cur_obj->getCollisionObjectType(), obj2.getCollisionObjectType());
34  if (!func) {
35  return -1;
36  }
37  func(*cur_obj, obj2, res, req);
38  if (res.collides()) return 0;
39  }
40  return 0;
41 }
42 
43 std::size_t collide_obj_tvobst(const CollisionObject &obj1,
44  const CollisionObject &obj2,
45  CollisionResult &res,
46  const CollisionRequest &req) {
47  return collide_tvobst_obj(obj2, obj1, res, req);
48 }
49 
50 std::size_t collide_tvobst_tvobst(const CollisionObject &obj1,
51  const CollisionObject &obj2,
52  CollisionResult &res,
53  const CollisionRequest &req) {
54  static solvers::DefaultSolver solver;
55  static CollisionFunctionMatrix matr_default(&solver);
56  const TimeVariantCollisionObject &tvobst1(
57  static_cast<const TimeVariantCollisionObject &>(obj1));
58  const TimeVariantCollisionObject &tvobst2(
59  static_cast<const TimeVariantCollisionObject &>(obj2));
60  int time_idx_start =
61  std::min(tvobst1.time_start_idx(), tvobst2.time_start_idx());
62  int time_idx_end = std::max(tvobst1.time_end_idx(), tvobst2.time_end_idx());
63 
64  for (int cc1 = time_idx_start; cc1 <= time_idx_end; cc1++) {
65  CollisionObjectConstPtr obst_1 = tvobst1.getObstacleAtTime(cc1);
66  CollisionObjectConstPtr obst_2 = tvobst2.getObstacleAtTime(cc1);
67 
68  if (!obst_1.get() || !obst_2.get()) {
69  continue;
70  }
71  if (obst_1->getCollisionObjectClass() == OBJ_CLASS_TVOBSTACLE ||
72  obst_2->getCollisionObjectClass() == OBJ_CLASS_TVOBSTACLE) {
73  return -1;
74  }
75 
76  collide_bool_func_t func = matr_default.getSolverBoolFunction(
77  obst_1->getCollisionObjectType(), obst_2->getCollisionObjectType());
78  if (!func) {
79  return -1;
80  }
81  func(*obst_1, *obst_2, res, req);
82  if (res.collides()) return 0;
83  }
84  return 0;
85 }
86 } // namespace solverDefault
87 
88 CollisionFunctionMatrix::CollisionFunctionMatrix(
89  const solvers::DefaultSolver *solver) {
90  memset(m_collide_bool_function, 0,
91  COL_OBJECT_TYPES_COUNT * COL_OBJECT_TYPES_COUNT);
92  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_AABB_BOX] =
93  solvers::solverPrimitive::collide_aabb_aabb;
94  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_OBB_BOX] =
95  solvers::solverPrimitive::collide_aabb_obb;
96  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_SPHERE] =
97  solvers::solverFCL::collide_obj_obj;
98  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_TRIANGLE] =
99  solvers::solverFCL::collide_obj_obj;
100  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_POINT] =
101  solvers::solverPrimitive::collide_aabb_point;
102  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_POLYGON] =
103  solvers::solverFCL::collide_obj_obj;
104  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_SHAPEGROUP] =
105  solvers::solverFCL::collide_obj_shape_group;
106  m_collide_bool_function[OBJ_TYPE_AABB_BOX][OBJ_TYPE_TVOBSTACLE] =
107  solvers::solverFCL::collide_obj_tvobst;
108 
109  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_AABB_BOX] =
110  solvers::solverPrimitive::collide_obb_aabb;
111  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_OBB_BOX] =
112  solvers::solverPrimitive::collide_obb_obb;
113  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_SPHERE] =
114  solvers::solverFCL::collide_obj_obj;
115  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_TRIANGLE] =
116  solvers::solverFCL::collide_obj_obj;
117  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_POINT] =
118  solvers::solverFCL::collide_obj_obj;
119  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_POLYGON] =
120  solvers::solverFCL::collide_obj_obj;
121  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_SHAPEGROUP] =
122  solvers::solverFCL::collide_obj_shape_group;
123  m_collide_bool_function[OBJ_TYPE_OBB_BOX][OBJ_TYPE_TVOBSTACLE] =
124  solvers::solverFCL::collide_obj_tvobst;
125 
126  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_AABB_BOX] =
127  solvers::solverFCL::collide_obj_obj;
128  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_OBB_BOX] =
129  solvers::solverFCL::collide_obj_obj;
130  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_SPHERE] =
131  solvers::solverFCL::collide_obj_obj;
132  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_TRIANGLE] =
133  solvers::solverFCL::collide_obj_obj;
134  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_POINT] =
135  solvers::solverPrimitive::collide_sphere_point;
136  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_POLYGON] =
137  solvers::solverFCL::collide_obj_obj;
138  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_SHAPEGROUP] =
139  solvers::solverFCL::collide_obj_shape_group;
140  m_collide_bool_function[OBJ_TYPE_SPHERE][OBJ_TYPE_TVOBSTACLE] =
141  solvers::solverFCL::collide_obj_tvobst;
142 
143  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_AABB_BOX] =
144  solvers::solverFCL::collide_obj_obj;
145  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_OBB_BOX] =
146  solvers::solverFCL::collide_obj_obj;
147  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_SPHERE] =
148  solvers::solverFCL::collide_obj_obj;
149  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_TRIANGLE] =
150  solvers::solverFCL::collide_obj_obj;
151  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_POINT] =
152  solvers::solverFCL::collide_obj_obj;
153  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_POLYGON] =
154  solvers::solverFCL::collide_obj_obj;
155  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_SHAPEGROUP] =
156  solvers::solverFCL::collide_obj_shape_group;
157  m_collide_bool_function[OBJ_TYPE_TRIANGLE][OBJ_TYPE_TVOBSTACLE] =
158  solvers::solverFCL::collide_obj_tvobst;
159 
160  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_AABB_BOX] =
161  solvers::solverPrimitive::collide_point_aabb;
162  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_OBB_BOX] =
163  solvers::solverFCL::collide_obj_obj;
164  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_SPHERE] =
165  solvers::solverPrimitive::collide_point_sphere;
166  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_TRIANGLE] =
167  solvers::solverFCL::collide_obj_obj;
168  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_POINT] =
169  solvers::solverPrimitive::collide_point_point;
170  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_POLYGON] =
171  solvers::solverFCL::collide_obj_obj;
172  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_SHAPEGROUP] =
173  solvers::solverFCL::collide_obj_shape_group;
174  m_collide_bool_function[OBJ_TYPE_POINT][OBJ_TYPE_TVOBSTACLE] =
175  solvers::solverFCL::collide_obj_tvobst;
176 
177  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_AABB_BOX] =
178  solvers::solverFCL::collide_obj_obj;
179  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_OBB_BOX] =
180  solvers::solverFCL::collide_obj_obj;
181  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_SPHERE] =
182  solvers::solverFCL::collide_obj_obj;
183  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_TRIANGLE] =
184  solvers::solverFCL::collide_obj_obj;
185  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_POINT] =
186  solvers::solverFCL::collide_obj_obj;
187  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_POLYGON] =
188  solvers::solverFCL::collide_obj_obj;
189  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_SHAPEGROUP] =
190  solvers::solverFCL::collide_obj_shape_group;
191  m_collide_bool_function[OBJ_TYPE_POLYGON][OBJ_TYPE_TVOBSTACLE] =
192  solvers::solverFCL::collide_obj_tvobst;
193 
194  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_AABB_BOX] =
195  solvers::solverFCL::collide_shape_group_obj;
196  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_OBB_BOX] =
197  solvers::solverFCL::collide_shape_group_obj;
198  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_SPHERE] =
199  solvers::solverFCL::collide_shape_group_obj;
200  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_TRIANGLE] =
201  solvers::solverFCL::collide_shape_group_obj;
202  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_POINT] =
203  solvers::solverFCL::collide_shape_group_obj;
204  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_POLYGON] =
205  solvers::solverFCL::collide_shape_group_obj;
206  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_SHAPEGROUP] =
207  solvers::solverFCL::collide_shape_group_shape_group;
208  m_collide_bool_function[OBJ_TYPE_SHAPEGROUP][OBJ_TYPE_TVOBSTACLE] =
209  solvers::solverFCL::collide_obj_tvobst;
210 
211  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_AABB_BOX] =
212  solvers::solverFCL::collide_tvobst_obj;
213  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_OBB_BOX] =
214  solvers::solverFCL::collide_tvobst_obj;
215  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_SPHERE] =
216  solvers::solverFCL::collide_tvobst_obj;
217  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_TRIANGLE] =
218  solvers::solverFCL::collide_tvobst_obj;
219  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_POINT] =
220  solvers::solverFCL::collide_tvobst_obj;
221  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_POLYGON] =
222  solvers::solverFCL::collide_tvobst_obj;
223  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_SHAPEGROUP] =
224  solvers::solverFCL::collide_tvobst_obj;
225  m_collide_bool_function[OBJ_TYPE_TVOBSTACLE][OBJ_TYPE_TVOBSTACLE] =
226  solvers::solverFCL::collide_tvobst_tvobst;
227 }
228 } // namespace solvers
229 } // namespace collision
collision::solvers::CollisionFunctionMatrix::getSolverBoolFunction
collide_bool_func_t getSolverBoolFunction(CollisionObjectType obj1_type, CollisionObjectType obj2_type) const
Definition: collision_solvers.h:23
collision::solvers::solverPrimitive::collide_point_aabb
std::size_t collide_point_aabb(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_primitive.cc:202
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::solvers::solverDefault::collide_tvobst_obj
std::size_t collide_tvobst_obj(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_default.cc:10
collision::solvers::solverPrimitive::collide_sphere_point
std::size_t collide_sphere_point(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_primitive.cc:148
collision::solvers::solverPrimitive::collide_aabb_aabb
std::size_t collide_aabb_aabb(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_primitive.cc:23
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::solverPrimitive::collide_aabb_obb
std::size_t collide_aabb_obb(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_primitive.cc:31
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::solverDefault::collide_tvobst_tvobst
std::size_t collide_tvobst_tvobst(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_default.cc:50
collision::solvers::solverPrimitive::collide_obb_obb
std::size_t collide_obb_obb(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_primitive.cc:74
time_variant_collision_object.h
collision::solvers::solverPrimitive::collide_obb_aabb
std::size_t collide_obb_aabb(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_primitive.cc:66
collision::CollisionRequest
Universal structure specifying collision request properties.
Definition: collision_requests.h:24
collision::solvers::solverPrimitive::collide_point_sphere
std::size_t collide_point_sphere(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_primitive.cc:219
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::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::solvers::solverDefault::collide_obj_tvobst
std::size_t collide_obj_tvobst(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_default.cc:43
collision::solvers::solverPrimitive::collide_point_point
std::size_t collide_point_point(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_primitive.cc:236
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
collision::solvers::solverPrimitive::collide_aabb_point
std::size_t collide_aabb_point(const CollisionObject &obj1, const CollisionObject &obj2, CollisionResult &res, const CollisionRequest &req)
Definition: collision_solver_primitive.cc:57
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_solver_primitive.h
collision::COL_OBJECT_TYPES_COUNT
constexpr int COL_OBJECT_TYPES_COUNT
Definition: collision_object_types.h:13