41 #include <../src/collision_node.h>
45 #include <../src/traits_traversal.h>
50 #ifdef HPP_FCL_HAS_OCTOMAP
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();
60 if (request.security_margin < 0)
62 "Negative security margin are not handled yet for Octree",
63 std::invalid_argument);
65 typename TraversalTraitsCollision<TypeA, TypeB>::CollisionTraversal_t node(
67 const TypeA* obj1 =
dynamic_cast<const TypeA*
>(o1);
68 const TypeB* obj2 =
dynamic_cast<const TypeB*
>(o2);
69 OcTreeSolver otsolver(nsolver);
72 collide(&node, request, result);
74 return result.numContacts();
80 template <
typename T_BVH,
typename T_SH>
82 enum {
Options = RelativeTransformationIsIdentity };
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 }; \
93 #undef BVH_SHAPE_DEFAULT_TO_ORIENTED
100 template <
typename T_BVH,
typename T_SH,
113 "Negative security margin are not handled yet for BVHModel",
114 std::invalid_argument);
116 if (_Options & RelativeTransformationIsIdentity)
117 return aligned(o1,
tf1, o2,
tf2, nsolver, request, result);
119 return oriented(o1,
tf1, o2,
tf2, nsolver, request, result);
130 MeshShapeCollisionTraversalNode<T_BVH, T_SH,
131 RelativeTransformationIsIdentity>
136 const T_SH* obj2 =
static_cast<const T_SH*
>(o2);
138 initialize(node, *obj1_tmp, tf1_tmp, *obj2,
tf2, nsolver, result);
153 MeshShapeCollisionTraversalNode<T_BVH, T_SH, 0> node(request);
155 const T_SH* obj2 =
static_cast<const T_SH*
>(o2);
168 template <
typename BV,
typename Shape>
180 const HF& height_field =
static_cast<const HF&
>(*o1);
181 const Shape& shape =
static_cast<const Shape&
>(*o2);
183 HeightFieldShapeCollisionTraversalNode<BV, Shape, 0> node(request);
192 template <
typename OrientedMeshCollisionTraversalNode,
typename T_BVH>
201 OrientedMeshCollisionTraversalNode node(request);
206 collide(&node, request, result);
213 template <
typename T_BVH>
220 MeshCollisionTraversalNode<T_BVH> node(request);
228 initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, result);
242 return details::orientedMeshCollide<MeshCollisionTraversalNodeOBB, OBB>(
243 o1,
tf1, o2,
tf2, request, result);
253 return details::orientedMeshCollide<MeshCollisionTraversalNodeOBBRSS, OBBRSS>(
254 o1,
tf1, o2,
tf2, request, result);
264 return details::orientedMeshCollide<MeshCollisionTraversalNodekIOS, kIOS>(
265 o1,
tf1, o2,
tf2, request, result);
268 template <
typename T_BVH>
274 return BVHCollide<T_BVH>(o1,
tf1, o2,
tf2, request, result);
290 &ShapeShapeCollide<Box, Halfspace>;
292 &ShapeShapeCollide<Box, Ellipsoid>;
296 &ShapeShapeCollide<Sphere, Sphere>;
298 &ShapeShapeCollide<Sphere, Capsule>;
301 &ShapeShapeCollide<Sphere, Cylinder>;
303 &ShapeShapeCollide<Sphere, ConvexBase>;
306 &ShapeShapeCollide<Sphere, Halfspace>;
308 &ShapeShapeCollide<Sphere, Ellipsoid>;
311 &ShapeShapeCollide<Ellipsoid, Box>;
313 &ShapeShapeCollide<Ellipsoid, Sphere>;
315 &ShapeShapeCollide<Ellipsoid, Capsule>;
317 &ShapeShapeCollide<Ellipsoid, Cone>;
319 &ShapeShapeCollide<Ellipsoid, Cylinder>;
321 &ShapeShapeCollide<Ellipsoid, ConvexBase>;
325 &ShapeShapeCollide<Ellipsoid, Ellipsoid>;
329 &ShapeShapeCollide<Capsule, Sphere>;
331 &ShapeShapeCollide<Capsule, Capsule>;
334 &ShapeShapeCollide<Capsule, Cylinder>;
336 &ShapeShapeCollide<Capsule, ConvexBase>;
338 &ShapeShapeCollide<Capsule, Plane>;
340 &ShapeShapeCollide<Capsule, Halfspace>;
342 &ShapeShapeCollide<Capsule, Ellipsoid>;
349 &ShapeShapeCollide<Cone, Cylinder>;
351 &ShapeShapeCollide<Cone, ConvexBase>;
354 &ShapeShapeCollide<Cone, Halfspace>;
356 &ShapeShapeCollide<Cone, Ellipsoid>;
360 &ShapeShapeCollide<Cylinder, Sphere>;
362 &ShapeShapeCollide<Cylinder, Capsule>;
364 &ShapeShapeCollide<Cylinder, Cone>;
366 &ShapeShapeCollide<Cylinder, Cylinder>;
368 &ShapeShapeCollide<Cylinder, ConvexBase>;
370 &ShapeShapeCollide<Cylinder, Plane>;
372 &ShapeShapeCollide<Cylinder, Halfspace>;
374 &ShapeShapeCollide<Cylinder, Ellipsoid>;
378 &ShapeShapeCollide<ConvexBase, Sphere>;
380 &ShapeShapeCollide<ConvexBase, Capsule>;
382 &ShapeShapeCollide<ConvexBase, Cone>;
384 &ShapeShapeCollide<ConvexBase, Cylinder>;
386 &ShapeShapeCollide<ConvexBase, ConvexBase>;
388 &ShapeShapeCollide<ConvexBase, Plane>;
390 &ShapeShapeCollide<ConvexBase, Halfspace>;
392 &ShapeShapeCollide<ConvexBase, Ellipsoid>;
397 &ShapeShapeCollide<Plane, Capsule>;
400 &ShapeShapeCollide<Plane, Cylinder>;
402 &ShapeShapeCollide<Plane, ConvexBase>;
405 &ShapeShapeCollide<Plane, Halfspace>;
409 &ShapeShapeCollide<Halfspace, Box>;
411 &ShapeShapeCollide<Halfspace, Sphere>;
413 &ShapeShapeCollide<Halfspace, Capsule>;
415 &ShapeShapeCollide<Halfspace, Cone>;
417 &ShapeShapeCollide<Halfspace, Cylinder>;
419 &ShapeShapeCollide<Halfspace, ConvexBase>;
421 &ShapeShapeCollide<Halfspace, Plane>;
423 &ShapeShapeCollide<Halfspace, Halfspace>;
615 #ifdef HPP_FCL_HAS_OCTOMAP
621 &OctreeCollide<OcTree, Cylinder>;
623 &OctreeCollide<OcTree, ConvexBase>;
626 &OctreeCollide<OcTree, Halfspace>;
628 &OctreeCollide<OcTree, Ellipsoid>;
635 &OctreeCollide<Cylinder, OcTree>;
637 &OctreeCollide<ConvexBase, OcTree>;
640 &OctreeCollide<Halfspace, OcTree>;
645 &OctreeCollide<OcTree, BVHModel<AABB> >;
647 &OctreeCollide<OcTree, BVHModel<OBB> >;
649 &OctreeCollide<OcTree, BVHModel<RSS> >;
651 &OctreeCollide<OcTree, BVHModel<OBBRSS> >;
653 &OctreeCollide<OcTree, BVHModel<kIOS> >;
655 &OctreeCollide<OcTree, BVHModel<KDOP<16> > >;
657 &OctreeCollide<OcTree, BVHModel<KDOP<18> > >;
659 &OctreeCollide<OcTree, BVHModel<KDOP<24> > >;
662 &OctreeCollide<BVHModel<AABB>,
OcTree>;
666 &OctreeCollide<BVHModel<OBBRSS>,
OcTree>;
668 &OctreeCollide<BVHModel<kIOS>,
OcTree>;
670 &OctreeCollide<BVHModel<KDOP<16> >,
OcTree>;
672 &OctreeCollide<BVHModel<KDOP<18> >,
OcTree>;
674 &OctreeCollide<BVHModel<KDOP<24> >,
OcTree>;