collision_func_matrix.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2015, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
39 
41 #include <../src/collision_node.h>
45 #include <../src/traits_traversal.h>
46 
47 namespace hpp {
48 namespace fcl {
49 
50 #ifdef HPP_FCL_HAS_OCTOMAP
51 
52 template <typename TypeA, typename TypeB>
53 std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3f& tf1,
54  const CollisionGeometry* o2, const Transform3f& tf2,
55  const GJKSolver* nsolver,
56  const CollisionRequest& request,
57  CollisionResult& result) {
58  if (request.isSatisfied(result)) return result.numContacts();
59 
60  if (request.security_margin < 0)
62  "Negative security margin are not handled yet for Octree",
63  std::invalid_argument);
64 
65  typename TraversalTraitsCollision<TypeA, TypeB>::CollisionTraversal_t node(
66  request);
67  const TypeA* obj1 = dynamic_cast<const TypeA*>(o1);
68  const TypeB* obj2 = dynamic_cast<const TypeB*>(o2);
69  OcTreeSolver otsolver(nsolver);
70 
71  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
72  collide(&node, request, result);
73 
74  return result.numContacts();
75 }
76 
77 #endif
78 
79 namespace details {
80 template <typename T_BVH, typename T_SH>
82  enum { Options = RelativeTransformationIsIdentity };
83 };
84 #define BVH_SHAPE_DEFAULT_TO_ORIENTED(bv) \
85  template <typename T_SH> \
86  struct bvh_shape_traits<bv, T_SH> { \
87  enum { Options = 0 }; \
88  }
93 #undef BVH_SHAPE_DEFAULT_TO_ORIENTED
94 } // namespace details
95 
100 template <typename T_BVH, typename T_SH,
102 struct HPP_FCL_LOCAL BVHShapeCollider {
103  static std::size_t collide(const CollisionGeometry* o1,
104  const Transform3f& tf1,
105  const CollisionGeometry* o2,
106  const Transform3f& tf2, const GJKSolver* nsolver,
107  const CollisionRequest& request,
108  CollisionResult& result) {
109  if (request.isSatisfied(result)) return result.numContacts();
110 
111  if (request.security_margin < 0)
113  "Negative security margin are not handled yet for BVHModel",
114  std::invalid_argument);
115 
116  if (_Options & RelativeTransformationIsIdentity)
117  return aligned(o1, tf1, o2, tf2, nsolver, request, result);
118  else
119  return oriented(o1, tf1, o2, tf2, nsolver, request, result);
120  }
121 
122  static std::size_t aligned(const CollisionGeometry* o1,
123  const Transform3f& tf1,
124  const CollisionGeometry* o2,
125  const Transform3f& tf2, const GJKSolver* nsolver,
126  const CollisionRequest& request,
127  CollisionResult& result) {
128  if (request.isSatisfied(result)) return result.numContacts();
129 
130  MeshShapeCollisionTraversalNode<T_BVH, T_SH,
131  RelativeTransformationIsIdentity>
132  node(request);
133  const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
134  BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
135  Transform3f tf1_tmp = tf1;
136  const T_SH* obj2 = static_cast<const T_SH*>(o2);
137 
138  initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, result);
139  fcl::collide(&node, request, result);
140 
141  delete obj1_tmp;
142  return result.numContacts();
143  }
144 
145  static std::size_t oriented(const CollisionGeometry* o1,
146  const Transform3f& tf1,
147  const CollisionGeometry* o2,
148  const Transform3f& tf2, const GJKSolver* nsolver,
149  const CollisionRequest& request,
150  CollisionResult& result) {
151  if (request.isSatisfied(result)) return result.numContacts();
152 
153  MeshShapeCollisionTraversalNode<T_BVH, T_SH, 0> node(request);
154  const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
155  const T_SH* obj2 = static_cast<const T_SH*>(o2);
156 
157  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result);
158  fcl::collide(&node, request, result);
159  return result.numContacts();
160  }
161 };
162 
168 template <typename BV, typename Shape>
169 struct HPP_FCL_LOCAL HeightFieldShapeCollider {
171 
172  static std::size_t collide(const CollisionGeometry* o1,
173  const Transform3f& tf1,
174  const CollisionGeometry* o2,
175  const Transform3f& tf2, const GJKSolver* nsolver,
176  const CollisionRequest& request,
177  CollisionResult& result) {
178  if (request.isSatisfied(result)) return result.numContacts();
179 
180  const HF& height_field = static_cast<const HF&>(*o1);
181  const Shape& shape = static_cast<const Shape&>(*o2);
182 
183  HeightFieldShapeCollisionTraversalNode<BV, Shape, 0> node(request);
184 
185  initialize(node, height_field, tf1, shape, tf2, nsolver, result);
186  fcl::collide(&node, request, result);
187  return result.numContacts();
188  }
189 };
190 
191 namespace details {
192 template <typename OrientedMeshCollisionTraversalNode, typename T_BVH>
194  const Transform3f& tf1,
195  const CollisionGeometry* o2,
196  const Transform3f& tf2,
197  const CollisionRequest& request,
198  CollisionResult& result) {
199  if (request.isSatisfied(result)) return result.numContacts();
200 
201  OrientedMeshCollisionTraversalNode node(request);
202  const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
203  const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
204 
205  initialize(node, *obj1, tf1, *obj2, tf2, result);
206  collide(&node, request, result);
207 
208  return result.numContacts();
209 }
210 
211 } // namespace details
212 
213 template <typename T_BVH>
214 std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1,
215  const CollisionGeometry* o2, const Transform3f& tf2,
216  const CollisionRequest& request,
217  CollisionResult& result) {
218  if (request.isSatisfied(result)) return result.numContacts();
219 
220  MeshCollisionTraversalNode<T_BVH> node(request);
221  const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
222  const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
223  BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
224  Transform3f tf1_tmp = tf1;
225  BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
226  Transform3f tf2_tmp = tf2;
227 
228  initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, result);
229  fcl::collide(&node, request, result);
230 
231  delete obj1_tmp;
232  delete obj2_tmp;
233 
234  return result.numContacts();
235 }
236 
237 template <>
238 std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const Transform3f& tf1,
239  const CollisionGeometry* o2, const Transform3f& tf2,
240  const CollisionRequest& request,
241  CollisionResult& result) {
242  return details::orientedMeshCollide<MeshCollisionTraversalNodeOBB, OBB>(
243  o1, tf1, o2, tf2, request, result);
244 }
245 
246 template <>
247 std::size_t BVHCollide<OBBRSS>(const CollisionGeometry* o1,
248  const Transform3f& tf1,
249  const CollisionGeometry* o2,
250  const Transform3f& tf2,
251  const CollisionRequest& request,
252  CollisionResult& result) {
253  return details::orientedMeshCollide<MeshCollisionTraversalNodeOBBRSS, OBBRSS>(
254  o1, tf1, o2, tf2, request, result);
255 }
256 
257 template <>
258 std::size_t BVHCollide<kIOS>(const CollisionGeometry* o1,
259  const Transform3f& tf1,
260  const CollisionGeometry* o2,
261  const Transform3f& tf2,
262  const CollisionRequest& request,
263  CollisionResult& result) {
264  return details::orientedMeshCollide<MeshCollisionTraversalNodekIOS, kIOS>(
265  o1, tf1, o2, tf2, request, result);
266 }
267 
268 template <typename T_BVH>
269 std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1,
270  const CollisionGeometry* o2, const Transform3f& tf2,
271  const GJKSolver* /*nsolver*/,
272  const CollisionRequest& request,
273  CollisionResult& result) {
274  return BVHCollide<T_BVH>(o1, tf1, o2, tf2, request, result);
275 }
276 
278  for (int i = 0; i < NODE_COUNT; ++i) {
279  for (int j = 0; j < NODE_COUNT; ++j) collision_matrix[i][j] = NULL;
280  }
281 
282  collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide<Box, Box>;
283  collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide<Box, Sphere>;
284  collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide<Box, Capsule>;
285  collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide<Box, Cone>;
286  collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide<Box, Cylinder>;
287  collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide<Box, ConvexBase>;
288  collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide<Box, Plane>;
289  collision_matrix[GEOM_BOX][GEOM_HALFSPACE] =
290  &ShapeShapeCollide<Box, Halfspace>;
291  collision_matrix[GEOM_BOX][GEOM_ELLIPSOID] =
292  &ShapeShapeCollide<Box, Ellipsoid>;
293 
294  collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide<Sphere, Box>;
295  collision_matrix[GEOM_SPHERE][GEOM_SPHERE] =
296  &ShapeShapeCollide<Sphere, Sphere>;
297  collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] =
298  &ShapeShapeCollide<Sphere, Capsule>;
299  collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide<Sphere, Cone>;
300  collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] =
301  &ShapeShapeCollide<Sphere, Cylinder>;
302  collision_matrix[GEOM_SPHERE][GEOM_CONVEX] =
303  &ShapeShapeCollide<Sphere, ConvexBase>;
304  collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide<Sphere, Plane>;
305  collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] =
306  &ShapeShapeCollide<Sphere, Halfspace>;
307  collision_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] =
308  &ShapeShapeCollide<Sphere, Ellipsoid>;
309 
310  collision_matrix[GEOM_ELLIPSOID][GEOM_BOX] =
311  &ShapeShapeCollide<Ellipsoid, Box>;
312  collision_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] =
313  &ShapeShapeCollide<Ellipsoid, Sphere>;
314  collision_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] =
315  &ShapeShapeCollide<Ellipsoid, Capsule>;
316  collision_matrix[GEOM_ELLIPSOID][GEOM_CONE] =
317  &ShapeShapeCollide<Ellipsoid, Cone>;
318  collision_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] =
319  &ShapeShapeCollide<Ellipsoid, Cylinder>;
320  collision_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] =
321  &ShapeShapeCollide<Ellipsoid, ConvexBase>;
322  // TODO Louis: Ellipsoid - Plane
323  // TODO Louis: Ellipsoid - Halfspace
324  collision_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] =
325  &ShapeShapeCollide<Ellipsoid, Ellipsoid>;
326 
327  collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide<Capsule, Box>;
328  collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] =
329  &ShapeShapeCollide<Capsule, Sphere>;
330  collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] =
331  &ShapeShapeCollide<Capsule, Capsule>;
332  collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide<Capsule, Cone>;
333  collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] =
334  &ShapeShapeCollide<Capsule, Cylinder>;
335  collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] =
336  &ShapeShapeCollide<Capsule, ConvexBase>;
337  collision_matrix[GEOM_CAPSULE][GEOM_PLANE] =
338  &ShapeShapeCollide<Capsule, Plane>;
339  collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] =
340  &ShapeShapeCollide<Capsule, Halfspace>;
341  collision_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] =
342  &ShapeShapeCollide<Capsule, Ellipsoid>;
343 
344  collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide<Cone, Box>;
345  collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide<Cone, Sphere>;
346  collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide<Cone, Capsule>;
347  collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide<Cone, Cone>;
348  collision_matrix[GEOM_CONE][GEOM_CYLINDER] =
349  &ShapeShapeCollide<Cone, Cylinder>;
350  collision_matrix[GEOM_CONE][GEOM_CONVEX] =
351  &ShapeShapeCollide<Cone, ConvexBase>;
352  collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide<Cone, Plane>;
353  collision_matrix[GEOM_CONE][GEOM_HALFSPACE] =
354  &ShapeShapeCollide<Cone, Halfspace>;
355  collision_matrix[GEOM_CONE][GEOM_ELLIPSOID] =
356  &ShapeShapeCollide<Cone, Ellipsoid>;
357 
358  collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide<Cylinder, Box>;
359  collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] =
360  &ShapeShapeCollide<Cylinder, Sphere>;
361  collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] =
362  &ShapeShapeCollide<Cylinder, Capsule>;
363  collision_matrix[GEOM_CYLINDER][GEOM_CONE] =
364  &ShapeShapeCollide<Cylinder, Cone>;
365  collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] =
366  &ShapeShapeCollide<Cylinder, Cylinder>;
367  collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] =
368  &ShapeShapeCollide<Cylinder, ConvexBase>;
369  collision_matrix[GEOM_CYLINDER][GEOM_PLANE] =
370  &ShapeShapeCollide<Cylinder, Plane>;
371  collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] =
372  &ShapeShapeCollide<Cylinder, Halfspace>;
373  collision_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] =
374  &ShapeShapeCollide<Cylinder, Ellipsoid>;
375 
376  collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide<ConvexBase, Box>;
377  collision_matrix[GEOM_CONVEX][GEOM_SPHERE] =
378  &ShapeShapeCollide<ConvexBase, Sphere>;
379  collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] =
380  &ShapeShapeCollide<ConvexBase, Capsule>;
381  collision_matrix[GEOM_CONVEX][GEOM_CONE] =
382  &ShapeShapeCollide<ConvexBase, Cone>;
383  collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] =
384  &ShapeShapeCollide<ConvexBase, Cylinder>;
385  collision_matrix[GEOM_CONVEX][GEOM_CONVEX] =
386  &ShapeShapeCollide<ConvexBase, ConvexBase>;
387  collision_matrix[GEOM_CONVEX][GEOM_PLANE] =
388  &ShapeShapeCollide<ConvexBase, Plane>;
389  collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] =
390  &ShapeShapeCollide<ConvexBase, Halfspace>;
391  collision_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] =
392  &ShapeShapeCollide<ConvexBase, Ellipsoid>;
393 
394  collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide<Plane, Box>;
395  collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide<Plane, Sphere>;
396  collision_matrix[GEOM_PLANE][GEOM_CAPSULE] =
397  &ShapeShapeCollide<Plane, Capsule>;
398  collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide<Plane, Cone>;
399  collision_matrix[GEOM_PLANE][GEOM_CYLINDER] =
400  &ShapeShapeCollide<Plane, Cylinder>;
401  collision_matrix[GEOM_PLANE][GEOM_CONVEX] =
402  &ShapeShapeCollide<Plane, ConvexBase>;
403  collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide<Plane, Plane>;
404  collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] =
405  &ShapeShapeCollide<Plane, Halfspace>;
406  // TODO Louis: Ellipsoid - Plane
407 
408  collision_matrix[GEOM_HALFSPACE][GEOM_BOX] =
409  &ShapeShapeCollide<Halfspace, Box>;
410  collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] =
411  &ShapeShapeCollide<Halfspace, Sphere>;
412  collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] =
413  &ShapeShapeCollide<Halfspace, Capsule>;
414  collision_matrix[GEOM_HALFSPACE][GEOM_CONE] =
415  &ShapeShapeCollide<Halfspace, Cone>;
416  collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] =
417  &ShapeShapeCollide<Halfspace, Cylinder>;
418  collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX] =
419  &ShapeShapeCollide<Halfspace, ConvexBase>;
420  collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] =
421  &ShapeShapeCollide<Halfspace, Plane>;
422  collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] =
423  &ShapeShapeCollide<Halfspace, Halfspace>;
424  // TODO Louis: Ellipsoid - Halfspace
425 
427  collision_matrix[BV_AABB][GEOM_SPHERE] =
429  collision_matrix[BV_AABB][GEOM_CAPSULE] =
432  collision_matrix[BV_AABB][GEOM_CYLINDER] =
434  collision_matrix[BV_AABB][GEOM_CONVEX] =
436  collision_matrix[BV_AABB][GEOM_PLANE] =
438  collision_matrix[BV_AABB][GEOM_HALFSPACE] =
440  collision_matrix[BV_AABB][GEOM_ELLIPSOID] =
442 
444  collision_matrix[BV_OBB][GEOM_SPHERE] =
446  collision_matrix[BV_OBB][GEOM_CAPSULE] =
449  collision_matrix[BV_OBB][GEOM_CYLINDER] =
451  collision_matrix[BV_OBB][GEOM_CONVEX] =
454  collision_matrix[BV_OBB][GEOM_HALFSPACE] =
456  collision_matrix[BV_OBB][GEOM_ELLIPSOID] =
458 
460  collision_matrix[BV_RSS][GEOM_SPHERE] =
462  collision_matrix[BV_RSS][GEOM_CAPSULE] =
465  collision_matrix[BV_RSS][GEOM_CYLINDER] =
467  collision_matrix[BV_RSS][GEOM_CONVEX] =
470  collision_matrix[BV_RSS][GEOM_HALFSPACE] =
472  collision_matrix[BV_RSS][GEOM_ELLIPSOID] =
474 
475  collision_matrix[BV_KDOP16][GEOM_BOX] =
477  collision_matrix[BV_KDOP16][GEOM_SPHERE] =
479  collision_matrix[BV_KDOP16][GEOM_CAPSULE] =
481  collision_matrix[BV_KDOP16][GEOM_CONE] =
483  collision_matrix[BV_KDOP16][GEOM_CYLINDER] =
485  collision_matrix[BV_KDOP16][GEOM_CONVEX] =
487  collision_matrix[BV_KDOP16][GEOM_PLANE] =
489  collision_matrix[BV_KDOP16][GEOM_HALFSPACE] =
491  collision_matrix[BV_KDOP16][GEOM_ELLIPSOID] =
493 
494  collision_matrix[BV_KDOP18][GEOM_BOX] =
496  collision_matrix[BV_KDOP18][GEOM_SPHERE] =
498  collision_matrix[BV_KDOP18][GEOM_CAPSULE] =
500  collision_matrix[BV_KDOP18][GEOM_CONE] =
502  collision_matrix[BV_KDOP18][GEOM_CYLINDER] =
504  collision_matrix[BV_KDOP18][GEOM_CONVEX] =
506  collision_matrix[BV_KDOP18][GEOM_PLANE] =
508  collision_matrix[BV_KDOP18][GEOM_HALFSPACE] =
510  collision_matrix[BV_KDOP18][GEOM_ELLIPSOID] =
512 
513  collision_matrix[BV_KDOP24][GEOM_BOX] =
515  collision_matrix[BV_KDOP24][GEOM_SPHERE] =
517  collision_matrix[BV_KDOP24][GEOM_CAPSULE] =
519  collision_matrix[BV_KDOP24][GEOM_CONE] =
521  collision_matrix[BV_KDOP24][GEOM_CYLINDER] =
523  collision_matrix[BV_KDOP24][GEOM_CONVEX] =
525  collision_matrix[BV_KDOP24][GEOM_PLANE] =
527  collision_matrix[BV_KDOP24][GEOM_HALFSPACE] =
529  collision_matrix[BV_KDOP24][GEOM_ELLIPSOID] =
531 
533  collision_matrix[BV_kIOS][GEOM_SPHERE] =
535  collision_matrix[BV_kIOS][GEOM_CAPSULE] =
538  collision_matrix[BV_kIOS][GEOM_CYLINDER] =
540  collision_matrix[BV_kIOS][GEOM_CONVEX] =
542  collision_matrix[BV_kIOS][GEOM_PLANE] =
544  collision_matrix[BV_kIOS][GEOM_HALFSPACE] =
546  collision_matrix[BV_kIOS][GEOM_ELLIPSOID] =
548 
549  collision_matrix[BV_OBBRSS][GEOM_BOX] =
551  collision_matrix[BV_OBBRSS][GEOM_SPHERE] =
553  collision_matrix[BV_OBBRSS][GEOM_CAPSULE] =
555  collision_matrix[BV_OBBRSS][GEOM_CONE] =
557  collision_matrix[BV_OBBRSS][GEOM_CYLINDER] =
559  collision_matrix[BV_OBBRSS][GEOM_CONVEX] =
561  collision_matrix[BV_OBBRSS][GEOM_PLANE] =
563  collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] =
565  collision_matrix[BV_OBBRSS][GEOM_ELLIPSOID] =
567 
568  collision_matrix[HF_AABB][GEOM_BOX] =
570  collision_matrix[HF_AABB][GEOM_SPHERE] =
572  collision_matrix[HF_AABB][GEOM_CAPSULE] =
574  collision_matrix[HF_AABB][GEOM_CONE] =
576  collision_matrix[HF_AABB][GEOM_CYLINDER] =
578  collision_matrix[HF_AABB][GEOM_CONVEX] =
580  collision_matrix[HF_AABB][GEOM_PLANE] =
582  collision_matrix[HF_AABB][GEOM_HALFSPACE] =
584  collision_matrix[HF_AABB][GEOM_ELLIPSOID] =
586 
587  collision_matrix[HF_OBBRSS][GEOM_BOX] =
589  collision_matrix[HF_OBBRSS][GEOM_SPHERE] =
591  collision_matrix[HF_OBBRSS][GEOM_CAPSULE] =
593  collision_matrix[HF_OBBRSS][GEOM_CONE] =
595  collision_matrix[HF_OBBRSS][GEOM_CYLINDER] =
597  collision_matrix[HF_OBBRSS][GEOM_CONVEX] =
599  collision_matrix[HF_OBBRSS][GEOM_PLANE] =
601  collision_matrix[HF_OBBRSS][GEOM_HALFSPACE] =
603  collision_matrix[HF_OBBRSS][GEOM_ELLIPSOID] =
605 
606  collision_matrix[BV_AABB][BV_AABB] = &BVHCollide<AABB>;
607  collision_matrix[BV_OBB][BV_OBB] = &BVHCollide<OBB>;
608  collision_matrix[BV_RSS][BV_RSS] = &BVHCollide<RSS>;
609  collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide<KDOP<16> >;
610  collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide<KDOP<18> >;
611  collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide<KDOP<24> >;
612  collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide<kIOS>;
613  collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS>;
614 
615 #ifdef HPP_FCL_HAS_OCTOMAP
616  collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OctreeCollide<OcTree, Box>;
617  collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OctreeCollide<OcTree, Sphere>;
618  collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OctreeCollide<OcTree, Capsule>;
619  collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OctreeCollide<OcTree, Cone>;
620  collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] =
621  &OctreeCollide<OcTree, Cylinder>;
622  collision_matrix[GEOM_OCTREE][GEOM_CONVEX] =
623  &OctreeCollide<OcTree, ConvexBase>;
624  collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OctreeCollide<OcTree, Plane>;
625  collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] =
626  &OctreeCollide<OcTree, Halfspace>;
627  collision_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] =
628  &OctreeCollide<OcTree, Ellipsoid>;
629 
630  collision_matrix[GEOM_BOX][GEOM_OCTREE] = &OctreeCollide<Box, OcTree>;
631  collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &OctreeCollide<Sphere, OcTree>;
632  collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &OctreeCollide<Capsule, OcTree>;
633  collision_matrix[GEOM_CONE][GEOM_OCTREE] = &OctreeCollide<Cone, OcTree>;
634  collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] =
635  &OctreeCollide<Cylinder, OcTree>;
636  collision_matrix[GEOM_CONVEX][GEOM_OCTREE] =
637  &OctreeCollide<ConvexBase, OcTree>;
638  collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &OctreeCollide<Plane, OcTree>;
639  collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] =
640  &OctreeCollide<Halfspace, OcTree>;
641 
642  collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OctreeCollide<OcTree, OcTree>;
643 
644  collision_matrix[GEOM_OCTREE][BV_AABB] =
645  &OctreeCollide<OcTree, BVHModel<AABB> >;
646  collision_matrix[GEOM_OCTREE][BV_OBB] =
647  &OctreeCollide<OcTree, BVHModel<OBB> >;
648  collision_matrix[GEOM_OCTREE][BV_RSS] =
649  &OctreeCollide<OcTree, BVHModel<RSS> >;
650  collision_matrix[GEOM_OCTREE][BV_OBBRSS] =
651  &OctreeCollide<OcTree, BVHModel<OBBRSS> >;
652  collision_matrix[GEOM_OCTREE][BV_kIOS] =
653  &OctreeCollide<OcTree, BVHModel<kIOS> >;
654  collision_matrix[GEOM_OCTREE][BV_KDOP16] =
655  &OctreeCollide<OcTree, BVHModel<KDOP<16> > >;
656  collision_matrix[GEOM_OCTREE][BV_KDOP18] =
657  &OctreeCollide<OcTree, BVHModel<KDOP<18> > >;
658  collision_matrix[GEOM_OCTREE][BV_KDOP24] =
659  &OctreeCollide<OcTree, BVHModel<KDOP<24> > >;
660 
661  collision_matrix[BV_AABB][GEOM_OCTREE] =
662  &OctreeCollide<BVHModel<AABB>, OcTree>;
663  collision_matrix[BV_OBB][GEOM_OCTREE] = &OctreeCollide<BVHModel<OBB>, OcTree>;
664  collision_matrix[BV_RSS][GEOM_OCTREE] = &OctreeCollide<BVHModel<RSS>, OcTree>;
665  collision_matrix[BV_OBBRSS][GEOM_OCTREE] =
666  &OctreeCollide<BVHModel<OBBRSS>, OcTree>;
667  collision_matrix[BV_kIOS][GEOM_OCTREE] =
668  &OctreeCollide<BVHModel<kIOS>, OcTree>;
669  collision_matrix[BV_KDOP16][GEOM_OCTREE] =
670  &OctreeCollide<BVHModel<KDOP<16> >, OcTree>;
671  collision_matrix[BV_KDOP18][GEOM_OCTREE] =
672  &OctreeCollide<BVHModel<KDOP<18> >, OcTree>;
673  collision_matrix[BV_KDOP24][GEOM_OCTREE] =
674  &OctreeCollide<BVHModel<KDOP<24> >, OcTree>;
675 #endif
676 }
677 // template struct CollisionFunctionMatrix;
678 } // namespace fcl
679 
680 } // namespace hpp
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
Ellipsoid centered at point zero.
std::size_t BVHCollide< kIOS >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const CollisionRequest &request, CollisionResult &result)
BVH_SHAPE_DEFAULT_TO_ORIENTED(OBB)
Cylinder along Z axis. The cylinder is defined at its centroid.
Main namespace.
tuple tf2
bool initialize(MeshCollisionTraversalNode< BV, RelativeTransformationIsIdentity > &node, BVHModel< BV > &model1, Transform3f &tf1, BVHModel< BV > &model2, Transform3f &tf2, CollisionResult &result, bool use_refit=false, bool refit_bottomup=false)
Initialize traversal node for collision between two meshes, given the current transforms.
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
static std::size_t aligned(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const CollisionRequest &request, CollisionResult &result)
size_t numContacts() const
number of contacts found
bool isSatisfied(const CollisionResult &result) const
Data structure depicting a height field given by the base grid dimensions and the elevation along the...
Definition: hfield.h:182
Octree is one type of collision geometry which can encode uncertainty information in the sensor data...
Definition: octree.h:53
std::size_t BVHCollide(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const CollisionRequest &request, CollisionResult &result)
request to the collision algorithm
Center at zero point, axis aligned box.
static std::size_t collide(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const CollisionRequest &request, CollisionResult &result)
static std::size_t collide(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const CollisionRequest &request, CollisionResult &result)
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Cone The base of the cone is at and the top is at .
std::size_t BVHCollide< OBB >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const CollisionRequest &request, CollisionResult &result)
Center at zero point sphere.
Capsule It is where is the distance between the point x and the capsule segment AB...
Base for convex polytope.
std::size_t orientedMeshCollide(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const CollisionRequest &request, CollisionResult &result)
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
static std::size_t oriented(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const CollisionRequest &request, CollisionResult &result)
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
std::size_t BVHCollide< OBBRSS >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const CollisionRequest &request, CollisionResult &result)
The geometry for the object for collision or distance computation.
Collider functor for HeightField data structure.
tuple tf1
#define HPP_FCL_THROW_PRETTY(message, exception)
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision.


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00