collision-inl.h
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-2016, 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 
38 #ifndef FCL_COLLISION_INL_H
39 #define FCL_COLLISION_INL_H
40 
42 
46 
47 namespace fcl
48 {
49 
50 //==============================================================================
51 extern template
52 FCL_EXPORT
53 std::size_t collide(
54  const CollisionObject<double>* o1,
55  const CollisionObject<double>* o2,
56  const CollisionRequest<double>& request,
57  CollisionResult<double>& result);
58 
59 //==============================================================================
60 extern template
61 FCL_EXPORT
62 std::size_t collide(
63  const CollisionGeometry<double>* o1,
64  const Transform3<double>& tf1,
65  const CollisionGeometry<double>* o2,
66  const Transform3<double>& tf2,
67  const CollisionRequest<double>& request,
68  CollisionResult<double>& result);
69 
70 //==============================================================================
71 template<typename GJKSolver>
73 {
75  return table;
76 }
77 
78 //==============================================================================
79 template <typename S, typename NarrowPhaseSolver>
80 FCL_EXPORT
81 std::size_t collide(
82  const CollisionObject<S>* o1,
83  const CollisionObject<S>* o2,
84  const NarrowPhaseSolver* nsolver,
85  const CollisionRequest<S>& request,
86  CollisionResult<S>& result)
87 {
88  return collide(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(),
89  nsolver, request, result);
90 }
91 
92 //==============================================================================
93 template <typename S, typename NarrowPhaseSolver>
94 FCL_EXPORT
95 std::size_t collide(
96  const CollisionGeometry<S>* o1,
97  const Transform3<S>& tf1,
98  const CollisionGeometry<S>* o2,
99  const Transform3<S>& tf2,
100  const NarrowPhaseSolver* nsolver_,
101  const CollisionRequest<S>& request,
102  CollisionResult<S>& result)
103 {
104  const NarrowPhaseSolver* nsolver = nsolver_;
105  if(!nsolver_)
106  nsolver = new NarrowPhaseSolver();
107 
108  const auto& looktable = getCollisionFunctionLookTable<NarrowPhaseSolver>();
109 
110  std::size_t res;
111  if(request.num_max_contacts == 0)
112  {
113  std::cerr << "Warning: should stop early as num_max_contact is " << request.num_max_contacts << " !\n";
114  res = 0;
115  }
116  else
117  {
118  OBJECT_TYPE object_type1 = o1->getObjectType();
119  OBJECT_TYPE object_type2 = o2->getObjectType();
120  NODE_TYPE node_type1 = o1->getNodeType();
121  NODE_TYPE node_type2 = o2->getNodeType();
122 
123  if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
124  {
125  if(!looktable.collision_matrix[node_type2][node_type1])
126  {
127  std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported\n";
128  res = 0;
129  }
130  else
131  res = looktable.collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result);
132  }
133  else
134  {
135  if(!looktable.collision_matrix[node_type1][node_type2])
136  {
137  std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported\n";
138  res = 0;
139  }
140  else
141  res = looktable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, request, result);
142  }
143  }
144 
145  if(!nsolver_)
146  delete nsolver;
147 
148  return res;
149 }
150 
151 //==============================================================================
152 template <typename S>
153 FCL_EXPORT
154 std::size_t collide(const CollisionObject<S>* o1, const CollisionObject<S>* o2,
155  const CollisionRequest<S>& request, CollisionResult<S>& result)
156 {
157  switch(request.gjk_solver_type)
158  {
159  case GST_LIBCCD:
160  {
162  solver.collision_tolerance = request.gjk_tolerance;
163  return collide(o1, o2, &solver, request, result);
164  }
165  case GST_INDEP:
166  {
168  solver.gjk_tolerance = request.gjk_tolerance;
169  solver.epa_tolerance = request.gjk_tolerance;
170  return collide(o1, o2, &solver, request, result);
171  }
172  default:
173  return -1; // error
174  }
175 }
176 
177 //==============================================================================
178 template <typename S>
179 FCL_EXPORT
180 std::size_t collide(
181  const CollisionGeometry<S>* o1,
182  const Transform3<S>& tf1,
183  const CollisionGeometry<S>* o2,
184  const Transform3<S>& tf2,
185  const CollisionRequest<S>& request,
186  CollisionResult<S>& result)
187 {
188  switch(request.gjk_solver_type)
189  {
190  case GST_LIBCCD:
191  {
193  solver.collision_tolerance = request.gjk_tolerance;
194  return collide(o1, tf1, o2, tf2, &solver, request, result);
195  }
196  case GST_INDEP:
197  {
199  solver.gjk_tolerance = request.gjk_tolerance;
200  solver.epa_tolerance = request.gjk_tolerance;
201  return collide(o1, tf1, o2, tf2, &solver, request, result);
202  }
203  default:
204  std::cerr << "Warning! Invalid GJK solver\n";
205  return -1; // error
206  }
207 }
208 
209 } // namespace fcl
210 
211 #endif
fcl::CollisionGeometry< double >
template class FCL_EXPORT CollisionGeometry< double >
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::CollisionRequest::gjk_solver_type
GJKSolverType gjk_solver_type
Enumeration indicating the GJK solver implementation to use.
Definition: collision_request.h:78
collision_func_matrix.h
fcl::CollisionRequest::num_max_contacts
size_t num_max_contacts
The maximum number of contacts that can be returned.
Definition: collision_request.h:59
fcl::getCollisionFunctionLookTable
detail::CollisionFunctionMatrix< GJKSolver > & getCollisionFunctionLookTable()
Definition: collision-inl.h:72
fcl::detail::GJKSolver_libccd
collision and distance solver based on libccd library.
Definition: gjk_solver_libccd.h:54
fcl::detail::GJKSolver_libccd::collision_tolerance
S collision_tolerance
the threshold used in GJK algorithm to stop collision iteration
Definition: gjk_solver_libccd.h:168
fcl::detail::GJKSolver_indep::epa_tolerance
S epa_tolerance
the threshold used in EPA to stop iteration
Definition: gjk_solver_indep.h:171
fcl::CollisionResult
collision result
Definition: collision_request.h:48
fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:58
gjk_solver_libccd.h
fcl::CollisionGeometry::getNodeType
virtual NODE_TYPE getNodeType() const
get the node type
Definition: collision_geometry-inl.h:79
fcl::GST_LIBCCD
@ GST_LIBCCD
Definition: gjk_solver_type.h:45
fcl::detail::GJKSolver_indep::gjk_tolerance
S gjk_tolerance
the threshold used in GJK to stop iteration
Definition: gjk_solver_indep.h:174
fcl::detail::GJKSolver_indep
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: gjk_solver_indep.h:54
fcl::CollisionRequest
Parameters for performing collision request.
Definition: collision_request.h:52
fcl::CollisionObject::collisionGeometry
const std::shared_ptr< const CollisionGeometry< S > > & collisionGeometry() const
get geometry from the object instance
Definition: collision_object-inl.h:243
fcl::collide
template FCL_EXPORT std::size_t collide(const CollisionObject< double > *o1, const CollisionObject< double > *o2, const CollisionRequest< double > &request, CollisionResult< double > &result)
fcl::OBJECT_TYPE
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_geometry.h:50
fcl::CollisionRequest::gjk_tolerance
Real gjk_tolerance
Numerical tolerance to use in the GJK algorithm. NOTE: The default value is currently set as 1e-6 to ...
Definition: collision_request.h:94
fcl::CollisionObject::getTransform
const Transform3< S > & getTransform() const
get object's transform
Definition: collision_object-inl.h:170
fcl::detail::CollisionFunctionMatrix
collision matrix stores the functions for collision between different types of objects and provides a...
Definition: collision_func_matrix.h:54
gjk_solver_indep.h
fcl::CollisionObject< double >
template class FCL_EXPORT CollisionObject< double >
fcl::OT_BVH
@ OT_BVH
Definition: collision_geometry.h:50
fcl::GST_INDEP
@ GST_INDEP
Definition: gjk_solver_type.h:45
fcl::CollisionGeometry::getObjectType
virtual OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_geometry-inl.h:72
fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_geometry.h:53
fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:51
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::OT_GEOM
@ OT_GEOM
Definition: collision_geometry.h:50
collision.h


fcl
Author(s):
autogenerated on Tue Jun 22 2021 07:27:49