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);
71 initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
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);
157 initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result);
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);
185 initialize(node, height_field, tf1, shape, tf2, nsolver, result);
192 template <
typename OrientedMeshCollisionTraversalNode,
typename T_BVH>
201 OrientedMeshCollisionTraversalNode node(request);
205 initialize(node, *obj1, tf1, *obj2, tf2, result);
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);
279 for (
int j = 0; j <
NODE_COUNT; ++j) collision_matrix[i][j] = NULL;
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>;
CollisionFunctionMatrix()
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.
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...
Octree is one type of collision geometry which can encode uncertainty information in the sensor data...
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 ...
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.
#define HPP_FCL_THROW_PRETTY(message, exception)
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision.